FMTS=magproto.o gpx.o geo.o gpsman.o mapsend.o mapsource.o \
gpsutil.o tiger.o pcx.o csv.o cetus.o gpspilot.o magnav.o \
- psp.o mxf.o holux.o
+ psp.o mxf.o holux.o garmin.o
+
+JEEPS=jeeps/gpsapp.o jeeps/gpscom.o jeeps/gpsfmt.o jeeps/gpsinput.o \
+ jeeps/gpsmath.o jeeps/gpsmem.o \
+ jeeps/gpsproj.o jeeps/gpsprot.o jeeps/gpsread.o \
+ jeeps/gpsrqst.o jeeps/gpssend.o jeeps/gpsserial.o jeeps/gpsutil.o
OBJS=main.o queue.o route.o waypt.o util.o vecs.o mkshort.o \
- coldsync/util.o coldsync/pdb.o $(FMTS)
+ coldsync/util.o coldsync/pdb.o $(GARMIN) $(JEEPS) $(FMTS)
all: gpsbabel
--- /dev/null
+This is from jeeps-0.1.3. This code is under GPL.
--- /dev/null
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef gps_h
+#define gps_h
+
+#include "gpsport.h"
+#include <time.h>
+
+#define FRAMING_ERROR -1
+#define PROTOCOL_ERROR -2
+#define HARDWARE_ERROR -3
+#define SERIAL_ERROR -4
+#define MEMORY_ERROR -5
+#define GPS_UNSUPPORTED -6
+#define INPUT_ERROR -7
+
+#define MAX_GPS_PACKET_SIZE 1024
+#define GPS_TIME_OUT 5
+
+#define gpsTrue 1
+#define gpsFalse 0
+
+#define DLE 0x10
+#define ETX 0x03
+
+
+extern int32 gps_errno;
+extern int32 gps_warning;
+extern int32 gps_error;
+extern int32 gps_user;
+extern int32 gps_show_bytes;
+
+
+typedef struct GPS_SPacket
+{
+ UC dle;
+ UC type;
+ UC n;
+ UC *data;
+ UC chk;
+ UC edle;
+ UC etx;
+ UC bytes; /* Actual number of bytes (for sending) */
+} GPS_OPacket, *GPS_PPacket;
+
+
+
+typedef struct GPS_SProduct_Data_Type
+{
+ int16 id;
+ int16 version;
+ char desc[MAX_GPS_PACKET_SIZE];
+} GPS_OProduct_Data_Type, *GPS_PProduct_Data_Type;
+
+
+
+
+typedef struct GPS_SPvt_Data_Type
+{
+ float alt;
+ float epe;
+ float eph;
+ float epv;
+ int16 fix;
+ double tow;
+ double lat;
+ double lon;
+ float east;
+ float north;
+ float up;
+ float msl_hght;
+ int16 leap_scnds;
+ int32 wn_days;
+} GPS_OPvt_Data, *GPS_PPvt_Data;
+
+
+
+typedef struct GPS_STrack
+{
+ double lat; /* Degrees */
+ double lon; /* Degrees */
+ time_t Time; /* Unix time */
+ float alt; /* Altitude */
+ float dpth; /* Depth */
+ int32 tnew; /* New track? */
+ int32 ishdr; /* Track header? */
+ int32 dspl; /* Display on map? */
+ int32 colour; /* Colour */
+ char trk_ident[256]; /* Track identifier */
+}
+GPS_OTrack, *GPS_PTrack;
+
+
+
+typedef struct GPS_SAlmanac
+{
+ UC svid;
+ int16 wn;
+ float toa;
+ float af0;
+ float af1;
+ float e;
+ float sqrta;
+ float m0;
+ float w;
+ float omg0;
+ float odot;
+ float i;
+ UC hlth;
+} GPS_OAlmanac, *GPS_PAlmanac;
+
+
+typedef struct GPS_SWay
+{
+ char ident[256];
+ double lat;
+ double lon;
+ char cmnt[256];
+ float dst;
+ int32 smbl;
+ int32 dspl;
+ char wpt_ident[256];
+ char lnk_ident[256];
+ UC subclass[18];
+ int32 colour;
+ char cc[2];
+ UC wpt_class;
+ int32 alt;
+ char city[24];
+ char state[2];
+ char name[30];
+ char facility[32];
+ char addr[52];
+ char cross_road[52];
+ int32 attr;
+ float dpth;
+ int32 idx;
+ int32 prot;
+ int32 isrte;
+ int32 rte_prot;
+ UC rte_num;
+ char rte_cmnt[20];
+ char rte_ident[256];
+ int32 islink;
+ int32 rte_link_class;
+ char rte_link_subclass[18];
+ char rte_link_ident[256];
+} GPS_OWay, *GPS_PWay;
+
+
+
+
+#include "gpsserial.h"
+#include "gpssend.h"
+#include "gpsread.h"
+#include "gpsutil.h"
+#include "gpsapp.h"
+#include "gpsprot.h"
+#include "gpscom.h"
+#include "gpsfmt.h"
+#include "gpsmath.h"
+#include "gpsnmea.h"
+#include "gpsmem.h"
+#include "gpsrqst.h"
+#include "gpsinput.h"
+#include "gpsproj.h"
+#include "gpsnmeafmt.h"
+#include "gpsnmeaget.h"
+
+time_t gps_save_time;
+double gps_save_lat;
+double gps_save_lon;
+extern int32 gps_save_id;
+extern double gps_save_version;
+extern char gps_save_string[GPS_ARB_LEN];
+
+
+extern struct COMMANDDATA COMMAND_ID[2];
+extern struct LINKDATA LINK_ID[3];
+extern struct GPS_MODEL_PROTOCOL GPS_MP[];
+
+extern char *gps_marine_sym[];
+extern char *gps_land_sym[];
+extern char *gps_aviation_sym[];
+extern char *gps_16_sym[];
+
+
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null
+/********************************************************************
+** @source JEEPS application and data functions
+**
+** @author Copyright (C) 1999 Alan Bleasby
+** @version 1.0
+** @modified Dec 28 1999 Alan Bleasby. First version
+** @@
+**
+** This library is free software; you can redistribute it and/or
+** modify it under the terms of the GNU Library General Public
+** License as published by the Free Software Foundation; either
+** version 2 of the License, or (at your option) any later version.
+**
+** This library is distributed in the hope that it will be useful,
+** but WITHOUT ANY WARRANTY; without even the implied warranty of
+** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+** Library General Public License for more details.
+**
+** You should have received a copy of the GNU Library General Public
+** License along with this library; if not, write to the
+** Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+** Boston, MA 02111-1307, USA.
+********************************************************************/
+#include "gps.h"
+#include <stdio.h>
+#include <time.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+
+static int32 GPS_A000(const char *port);
+static void GPS_A001(GPS_PPacket packet);
+
+
+static void GPS_A500_Translate(UC *s, GPS_PAlmanac *alm);
+static void GPS_A500_Encode(UC *s, GPS_PAlmanac alm);
+static void GPS_A300_Translate(UC *s, GPS_PTrack *trk);
+static void GPS_A300_Encode(UC *s, GPS_PTrack trk);
+
+
+static void GPS_D100_Get(GPS_PWay *way, UC *s);
+static void GPS_D101_Get(GPS_PWay *way, UC *s);
+static void GPS_D102_Get(GPS_PWay *way, UC *s);
+static void GPS_D103_Get(GPS_PWay *way, UC *s);
+static void GPS_D104_Get(GPS_PWay *way, UC *s);
+static void GPS_D105_Get(GPS_PWay *way, UC *s);
+static void GPS_D106_Get(GPS_PWay *way, UC *s);
+static void GPS_D107_Get(GPS_PWay *way, UC *s);
+static void GPS_D108_Get(GPS_PWay *way, UC *s);
+static void GPS_D150_Get(GPS_PWay *way, UC *s);
+static void GPS_D151_Get(GPS_PWay *way, UC *s);
+static void GPS_D152_Get(GPS_PWay *way, UC *s);
+static void GPS_D154_Get(GPS_PWay *way, UC *s);
+static void GPS_D155_Get(GPS_PWay *way, UC *s);
+
+static void GPS_D100_Send(UC *data, GPS_PWay way, int32 *len);
+static void GPS_D101_Send(UC *data, GPS_PWay way, int32 *len);
+static void GPS_D102_Send(UC *data, GPS_PWay way, int32 *len);
+static void GPS_D103_Send(UC *data, GPS_PWay way, int32 *len);
+static void GPS_D104_Send(UC *data, GPS_PWay way, int32 *len);
+static void GPS_D105_Send(UC *data, GPS_PWay way, int32 *len);
+static void GPS_D106_Send(UC *data, GPS_PWay way, int32 *len);
+static void GPS_D107_Send(UC *data, GPS_PWay way, int32 *len);
+static void GPS_D108_Send(UC *data, GPS_PWay way, int32 *len);
+static void GPS_D150_Send(UC *data, GPS_PWay way, int32 *len);
+static void GPS_D151_Send(UC *data, GPS_PWay way, int32 *len);
+static void GPS_D152_Send(UC *data, GPS_PWay way, int32 *len);
+static void GPS_D154_Send(UC *data, GPS_PWay way, int32 *len);
+static void GPS_D155_Send(UC *data, GPS_PWay way, int32 *len);
+
+static void GPS_D200_Get(GPS_PWay *way, UC *s);
+static void GPS_D201_Get(GPS_PWay *way, UC *s);
+static void GPS_D202_Get(GPS_PWay *way, UC *s);
+static void GPS_D210_Get(GPS_PWay *way, UC *s);
+static void GPS_D200_Send(UC *data, GPS_PWay way, int32 *len);
+static void GPS_D201_Send(UC *data, GPS_PWay way, int32 *len);
+static void GPS_D202_Send(UC *data, GPS_PWay way, int32 *len);
+static void GPS_D210_Send(UC *data, GPS_PWay way, int32 *len);
+
+static void GPS_D400_Get(GPS_PWay *way, UC *s);
+static void GPS_D403_Get(GPS_PWay *way, UC *s);
+static void GPS_D450_Get(GPS_PWay *way, UC *s);
+static void GPS_D400_Send(UC *data, GPS_PWay way, int32 *len);
+static void GPS_D403_Send(UC *data, GPS_PWay way, int32 *len);
+static void GPS_D450_Send(UC *data, GPS_PWay way, int32 *len);
+
+static int32 GPS_D500_Get(GPS_PAlmanac *alm, int32 entries, int32 fd);
+static int32 GPS_D501_Get(GPS_PAlmanac *alm, int32 entries, int32 fd);
+static int32 GPS_D550_Get(GPS_PAlmanac *alm, int32 entries, int32 fd);
+static int32 GPS_D551_Get(GPS_PAlmanac *alm, int32 entries, int32 fd);
+static void GPS_D500_Send(UC *data, GPS_PAlmanac alm);
+static void GPS_D501_Send(UC *data, GPS_PAlmanac alm);
+static void GPS_D550_Send(UC *data, GPS_PAlmanac alm);
+static void GPS_D551_Send(UC *data, GPS_PAlmanac alm);
+
+
+int32 gps_save_id;
+double gps_save_version;
+char gps_save_string[GPS_ARB_LEN];
+
+
+/* @func GPS_Init ******************************************************
+**
+** Initialise GPS communication
+** Get capabilities and store time lat/lon in case GPS requests
+** it later.
+** Find endian nature of hardware and store
+**
+** @param [r] port [const char *] serial port
+**
+** @return [int32] 1 if success -ve if error
+************************************************************************/
+int32 GPS_Init(const char *port)
+{
+ int32 ret;
+
+ (void) GPS_Util_Little();
+ ret = GPS_A000(port);
+ if(ret<0) return ret;
+
+ gps_save_time = GPS_Command_Get_Time(port);
+ if(!gps_save_time) {
+ return FRAMING_ERROR;
+ }
+ return GPS_Command_Get_Position(port,&gps_save_lat,&gps_save_lon);
+}
+
+
+/* @funcstatic GPS_A000 ************************************************
+**
+** Return product ID, version and description. Turn off PVT transfer
+**
+** @param [r] port [const char *] serial port
+**
+** @return [int32] 1 if success -ve if error
+************************************************************************/
+static int32 GPS_A000(const char *port)
+{
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int16 version;
+ int16 id;
+ char tstr[256];
+
+ if(!GPS_Serial_On(port, &fd))
+ return gps_errno;
+
+ if(!GPS_Serial_Flush(fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+ GPS_Make_Packet(&tra, LINK_ID[0].Pid_Product_Rqst,NULL,0);
+ if(!GPS_Write_Packet(fd,tra))
+ return SERIAL_ERROR;
+
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ return SERIAL_ERROR;
+
+ GPS_Packet_Read(fd, &rec);
+ GPS_Send_Ack(fd, &tra, &rec);
+
+ id = GPS_Util_Get_Short(rec->data);
+ version = GPS_Util_Get_Short((rec->data)+2);
+
+ (void) strcpy(gps_save_string,(char *)rec->data+4);
+ GPS_User((char *)rec->data+4);
+ (void) sprintf(tstr,"ID:\t\t%d\n",id);
+ gps_save_id = id;
+ GPS_User(tstr);
+ gps_save_version = (double)((double)version/(double)100.);
+ (void) sprintf(tstr,
+ "Version:\t%.2f\n",gps_save_version);
+ GPS_User(tstr);
+
+
+
+
+ gps_date_time_transfer = pA600;
+ gps_date_time_type = pD600; /* All models so far */
+ gps_position_transfer = pA700;
+ gps_position_type = pD700; /* All models so far */
+ gps_pvt_transfer = -1;
+ gps_pvt_type = -1;
+ gps_prx_waypt_transfer = -1;
+ gps_prx_waypt_type = -1;
+ gps_trk_transfer = -1;
+ gps_trk_type = -1;
+ gps_trk_hdr_type = -1;
+ gps_rte_link_type = -1;
+
+ if(!GPS_Serial_Wait(fd))
+ {
+ GPS_Warning("A001 protocol not supported");
+ id = GPS_Protocol_Version_Change(id,version);
+ if(GPS_Protocol_Table_Set(id)<0)
+ return GPS_UNSUPPORTED;
+ }
+ else
+ {
+ (void) GPS_Packet_Read(fd, &rec);
+ GPS_Send_Ack(fd, &tra, &rec);
+ GPS_A001(rec);
+ }
+
+
+ /* Make sure PVT is off as some GPS' have it on by default */
+ if(gps_pvt_transfer != -1)
+ GPS_A800_Off(port,&fd);
+
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return 1;
+}
+
+
+
+
+/* @funcstatic GPS_A001 ************************************************
+**
+** Extract protocol capabilities
+** This routine could do with re-writing. It's too long and obtuse.
+**
+** @param [r] packet [GPS_PPacket] A001 protocol packet
+**
+** @return [void]
+************************************************************************/
+static void GPS_A001(GPS_PPacket packet)
+{
+ int32 entries;
+ int32 i;
+ UC *p;
+ US tag;
+ US data;
+ US lasta=0;
+
+ gps_link_type = -1;
+ gps_device_command = -1;
+ gps_waypt_transfer = -1;
+ gps_waypt_type = -1;
+ gps_route_transfer = -1;
+ gps_rte_hdr_type = -1;
+ gps_rte_type = -1;
+ gps_trk_transfer = -1;
+ gps_trk_type = -1;
+ gps_prx_waypt_transfer = -1;
+ gps_prx_waypt_type = -1;
+ gps_almanac_transfer = -1;
+ gps_almanac_type = -1;
+
+ entries = packet->n / 3;
+ p = packet->data;
+
+ for(i=0;i<entries;++i,p+=3)
+ {
+ tag = *p;
+ data = GPS_Util_Get_Short(p+1);
+
+ /* Only one type of P[hysical] so far */
+ if(tag == 'P')
+ {
+ if(data!=0)
+ GPS_Protocol_Error(tag,data);
+ continue;
+ }
+
+ if(tag == 'L')
+ {
+ gps_link_type = data;
+ continue;
+ }
+
+ if(tag == 'A')
+ {
+ lasta = data;
+ if(data<100)
+ {
+ if(data==10)
+ gps_device_command = pA010-10;
+ else if(data==11)
+ gps_device_command = pA011-10;
+ else
+ GPS_Protocol_Error(tag,data);
+ continue;
+ }
+ else if(data<200)
+ {
+ if(data!=100)
+ GPS_Protocol_Error(tag,data);
+ else
+ gps_waypt_transfer = pA100;
+ continue;
+ }
+ else if(data<300)
+ {
+ if(data==200)
+ gps_route_transfer = pA200;
+ else if(data==201)
+ gps_route_transfer = pA201;
+ else
+ GPS_Protocol_Error(tag,data);
+ continue;
+ }
+ else if(data<400)
+ {
+ if(data==300)
+ gps_trk_transfer = pA300;
+ else if(data==301)
+ gps_trk_transfer = pA301;
+ else
+ GPS_Protocol_Error(tag,data);
+ continue;
+ }
+ else if(data<500)
+ {
+ if(data!=400)
+ GPS_Protocol_Error(tag,data);
+ else
+ gps_prx_waypt_transfer = pA400;
+ continue;
+ }
+ else if(data<600)
+ {
+ if(data!=500)
+ GPS_Protocol_Error(tag,data);
+ else
+ gps_almanac_transfer = pA500;
+ continue;
+ }
+ else if(data<700)
+ {
+ if(data!=600)
+ GPS_Protocol_Error(tag,data);
+ else
+ gps_date_time_transfer = pA600;
+ continue;
+ }
+ else if(data<800)
+ {
+ if(data!=700)
+ GPS_Protocol_Error(tag,data);
+ gps_position_transfer = pA700;
+ continue;
+ }
+ else if(data<900)
+ {
+ if(data!=800)
+ GPS_Protocol_Error(tag,data);
+ else
+ gps_pvt_transfer = pA800;
+ continue;
+ }
+ else
+ {
+ GPS_Protocol_Error(tag,data);
+ }
+ }
+
+ if(tag == 'D')
+ {
+ if(lasta<200)
+ {
+ if(data<109 && data>=100)
+ {
+ gps_waypt_type = data;
+ continue;
+ }
+ if(data<153 && data>=150)
+ {
+ gps_waypt_type = data;
+ continue;
+ }
+ if(data<156 && data>=154)
+ {
+ gps_waypt_type = data;
+ continue;
+ }
+ else
+ GPS_Protocol_Error(tag,data);
+ }
+
+
+ else if(lasta<300)
+ {
+ if(data>=200 && data <=202)
+ {
+ gps_rte_hdr_type = data;
+ continue;
+ }
+ if(data==210)
+ {
+ gps_rte_link_type = data;
+ continue;
+ }
+
+ if(data<109 && data>=100)
+ {
+ gps_rte_type = data;
+ continue;
+ }
+ if(data<153 && data>=150)
+ {
+ gps_rte_type = data;
+ continue;
+ }
+ if(data<156 && data>=154)
+ {
+ gps_rte_type = data;
+ continue;
+ }
+ if(data<451)
+ {
+ if(data==400)
+ gps_rte_type = pD400;
+ else if(data==403)
+ gps_rte_type = pD403;
+ else if(data==450)
+ gps_rte_type = pD450;
+ else
+ GPS_Protocol_Error(tag,data);
+ continue;
+ }
+ }
+
+ else if(lasta<400)
+ {
+ if(data==300)
+ gps_trk_type = pD300;
+ else if(data==301)
+ gps_trk_type = pD301;
+ else if(data==310)
+ gps_trk_hdr_type = pD310;
+ else
+ GPS_Protocol_Error(tag,data);
+ continue;
+ }
+
+
+ else if(lasta<500)
+ {
+ if(data<109 && data>=100)
+ {
+ gps_prx_waypt_type = data;
+ continue;
+ }
+ if(data<153 && data>=150)
+ {
+ gps_prx_waypt_type = data;
+ continue;
+ }
+ if(data<156 && data>=154)
+ {
+ gps_prx_waypt_type = data;
+ continue;
+ }
+ if(data<451)
+ {
+ if(data==400)
+ gps_prx_waypt_type = pD400;
+ else if(data==403)
+ gps_prx_waypt_type = pD403;
+ else if(data==450)
+ gps_prx_waypt_type = pD450;
+ else
+ GPS_Protocol_Error(tag,data);
+ continue;
+ }
+ }
+
+ else if(lasta<600)
+ {
+ if(data==500)
+ gps_almanac_type = pD500;
+ else if(data==501)
+ gps_almanac_type = pD501;
+ else if(data==550)
+ gps_almanac_type = pD550;
+ else if(data==551)
+ gps_almanac_type = pD551;
+ else
+ GPS_Protocol_Error(tag,data);
+ continue;
+ }
+
+ else if(lasta<700)
+ {
+ if(data!=600)
+ GPS_Protocol_Error(tag,data);
+ else
+ gps_date_time_type = pD600;
+ continue;
+ }
+ else if(lasta<800)
+ {
+ if(data!=700)
+ GPS_Protocol_Error(tag,data);
+ else
+ gps_position_type = pD700;
+ continue;
+ }
+ else if(lasta<900)
+ {
+ if(data!=800)
+ GPS_Protocol_Error(tag,data);
+ else
+ gps_pvt_type = pD800;
+ continue;
+ }
+
+
+ }
+ }
+
+ return;
+}
+
+
+
+
+/* @func GPS_A100_Get ******************************************************
+**
+** Get waypoint data from GPS
+**
+** @param [r] port [const char *] serial port
+** @param [w] way [GPS_PWay **] waypoint array
+**
+** @return [int32] number of waypoint entries
+************************************************************************/
+int32 GPS_A100_Get(const char *port, GPS_PWay **way)
+{
+ static UC data[2];
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 n;
+ int32 i;
+
+
+ if(!GPS_Serial_On(port,&fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+ GPS_Util_Put_Short(data,
+ COMMAND_ID[gps_device_command].Cmnd_Transfer_Wpt);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
+ data,2);
+
+ if(!GPS_Write_Packet(fd,tra))
+ {
+ GPS_Error("A100_Get: Cannot write packet");
+ return FRAMING_ERROR;
+ }
+
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A100_Get: No acknowledge");
+ return FRAMING_ERROR;
+ }
+
+ GPS_Packet_Read(fd, &rec);
+ GPS_Send_Ack(fd, &tra, &rec);
+
+ n = GPS_Util_Get_Short(rec->data);
+
+ if(n)
+ if(!((*way)=(GPS_PWay *)malloc(n*sizeof(GPS_PWay))))
+ {
+ GPS_Error("A100_Get: Insufficient memory");
+ return MEMORY_ERROR;
+ }
+
+ for(i=0;i<n;++i)
+ {
+ if(!((*way)[i]=GPS_Way_New()))
+ return MEMORY_ERROR;
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ switch(gps_waypt_type)
+ {
+ case pD100:
+ GPS_D100_Get(&((*way)[i]),rec->data);
+ break;
+ case pD101:
+ GPS_D101_Get(&((*way)[i]),rec->data);
+ break;
+ case pD102:
+ GPS_D102_Get(&((*way)[i]),rec->data);
+ break;
+ case pD103:
+ GPS_D103_Get(&((*way)[i]),rec->data);
+ break;
+ case pD104:
+ GPS_D104_Get(&((*way)[i]),rec->data);
+ break;
+ case pD105:
+ GPS_D105_Get(&((*way)[i]),rec->data);
+ break;
+ case pD106:
+ GPS_D106_Get(&((*way)[i]),rec->data);
+ break;
+ case pD107:
+ GPS_D107_Get(&((*way)[i]),rec->data);
+ break;
+ case pD108:
+ GPS_D108_Get(&((*way)[i]),rec->data);
+ break;
+ case pD150:
+ GPS_D150_Get(&((*way)[i]),rec->data);
+ break;
+ case pD151:
+ GPS_D151_Get(&((*way)[i]),rec->data);
+ break;
+ case pD152:
+ GPS_D152_Get(&((*way)[i]),rec->data);
+ break;
+ case pD154:
+ GPS_D154_Get(&((*way)[i]),rec->data);
+ break;
+ case pD155:
+ GPS_D155_Get(&((*way)[i]),rec->data);
+ break;
+ default:
+ GPS_Error("A100_GET: Unknown waypoint protocol");
+ return PROTOCOL_ERROR;
+ }
+ }
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt)
+ {
+ GPS_Error("A100_GET: Error transferring waypoints");
+ return FRAMING_ERROR;
+ }
+
+ if(i != n)
+ {
+ GPS_Error("A100_GET: Waypoint entry number mismatch");
+ return FRAMING_ERROR;
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return n;
+}
+
+
+
+
+
+/* @func GPS_A100_Send **************************************************
+**
+** Send waypoints to GPS
+**
+** @param [r] port [const char *] serial port
+** @param [r] trk [GPS_PWay *] waypoint array
+** @param [r] n [int32] number of waypoint entries
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_A100_Send(const char *port, GPS_PWay *way, int32 n)
+{
+ UC data[GPS_ARB_LEN];
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 i;
+ int32 len;
+
+ if(!GPS_Serial_On(port,&fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+ GPS_Util_Put_Short(data,n);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("Waypoint start data not acknowledged");
+ return gps_errno;
+ }
+
+
+ for(i=0;i<n;++i)
+ {
+ switch(gps_waypt_type)
+ {
+ case pD100:
+ GPS_D100_Send(data,way[i],&len);
+ break;
+ case pD101:
+ GPS_D101_Send(data,way[i],&len);
+ break;
+ case pD102:
+ GPS_D102_Send(data,way[i],&len);
+ break;
+ case pD103:
+ GPS_D103_Send(data,way[i],&len);
+ break;
+ case pD104:
+ GPS_D104_Send(data,way[i],&len);
+ break;
+ case pD105:
+ GPS_D105_Send(data,way[i],&len);
+ break;
+ case pD106:
+ GPS_D106_Send(data,way[i],&len);
+ break;
+ case pD107:
+ GPS_D107_Send(data,way[i],&len);
+ break;
+ case pD108:
+ GPS_D108_Send(data,way[i],&len);
+ break;
+ case pD150:
+ GPS_D150_Send(data,way[i],&len);
+ break;
+ case pD151:
+ GPS_D151_Send(data,way[i],&len);
+ break;
+ case pD152:
+ GPS_D152_Send(data,way[i],&len);
+ break;
+ case pD154:
+ GPS_D154_Send(data,way[i],&len);
+ break;
+ case pD155:
+ GPS_D155_Send(data,way[i],&len);
+ break;
+ default:
+ GPS_Error("Unknown waypoint protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Wpt_Data,
+ data,len);
+
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A100_Send: Pid_Wpt_Data not acknowledged");
+ return gps_errno;
+ }
+ }
+
+ GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Wpt);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("Waypoint complete data not acknowledged");
+ return gps_errno;
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return 1;
+}
+
+
+
+/* @funcstatic GPS_D100_Get *********************************************
+**
+** Get waypoint data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D100_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ int32 i;
+
+ p=s;
+
+ (*way)->prot = 100;
+ for(i=0;i<6;++i) (*way)->ident[i] = *p++;
+
+ (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ p+=sizeof(int32);
+
+ for(i=0;i<40;++i) (*way)->cmnt[i] = *p++;
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D101_Get *********************************************
+**
+** Get waypoint data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D101_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ int32 i;
+
+ p=s;
+
+ (*way)->prot = 101;
+ for(i=0;i<6;++i) (*way)->ident[i] = *p++;
+
+ (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ p+=sizeof(int32);
+
+ for(i=0;i<40;++i) (*way)->cmnt[i] = *p++;
+
+ (*way)->dst = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*way)->smbl = *p;
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D102_Get ********************************************
+**
+** Get waypoint data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D102_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ int32 i;
+
+ p=s;
+
+ (*way)->prot = 102;
+ for(i=0;i<6;++i) (*way)->ident[i] = *p++;
+
+ (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ p+=sizeof(int32);
+
+ for(i=0;i<40;++i) (*way)->cmnt[i] = *p++;
+
+ (*way)->dst = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*way)->smbl = GPS_Util_Get_Short(p);
+
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D103_Get *********************************************
+**
+** Get waypoint data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D103_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ int32 i;
+
+ p=s;
+
+ (*way)->prot = 103;
+ for(i=0;i<6;++i) (*way)->ident[i] = *p++;
+
+ (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ p+=sizeof(int32);
+
+ for(i=0;i<40;++i) (*way)->cmnt[i] = *p++;
+
+ (*way)->smbl = *p++;
+ (*way)->dspl = *p;
+
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D104_Get ********************************************
+**
+** Get waypoint data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D104_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ int32 i;
+
+ p=s;
+
+ (*way)->prot = 104;
+ for(i=0;i<6;++i) (*way)->ident[i] = *p++;
+
+ (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ p+=sizeof(int32);
+
+ for(i=0;i<40;++i) (*way)->cmnt[i] = *p++;
+
+ (*way)->dst = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*way)->smbl = GPS_Util_Get_Short(p);
+ p+=sizeof(int16);
+
+ (*way)->dspl = *p;
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D105_Get ********************************************
+**
+** Get waypoint data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D105_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ UC *q;
+
+ p=s;
+
+ (*way)->prot = 105;
+
+ (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->smbl = GPS_Util_Get_Short(p);
+ p+=sizeof(int16);
+
+ q = (UC *) (*way)->wpt_ident;
+ while((*q++ = *p++));
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D106_Get ********************************************
+**
+** Get waypoint data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+void GPS_D106_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ UC *q;
+ int32 i;
+
+ p=s;
+
+ (*way)->prot = 106;
+
+ (*way)->wpt_class = *p++;
+
+ for(i=0;i<13;++i) (*way)->subclass[i] = *p++;
+
+ (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->smbl = GPS_Util_Get_Short(p);
+ p+=sizeof(int16);
+
+ q = (UC *) (*way)->wpt_ident;
+ while((*q++ = *p++));
+ q = (UC *) (*way)->lnk_ident;
+ while((*q++ = *p++));
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D107_Get ********************************************
+**
+** Get waypoint data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D107_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ int32 i;
+
+ p=s;
+
+ (*way)->prot = 107;
+ for(i=0;i<6;++i) (*way)->ident[i] = *p++;
+
+ (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ p+=sizeof(int32);
+
+ for(i=0;i<40;++i) (*way)->cmnt[i] = *p++;
+
+ (*way)->smbl = *p++;
+ (*way)->dspl = *p++;
+
+ (*way)->dst = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*way)->colour = *p++;
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D108_Get ********************************************
+**
+** Get waypoint data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D108_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ UC *q;
+
+ int32 i;
+
+ p=s;
+
+ (*way)->prot = 108;
+
+ (*way)->wpt_class = *p++;
+ (*way)->colour = *p++;
+ (*way)->dspl = *p++;
+ (*way)->attr = *p++;
+ (*way)->smbl = GPS_Util_Get_Short(p);
+ p+=sizeof(int16);
+ for(i=0;i<18;++i) (*way)->subclass[i] = *p++;
+
+ (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->alt = (int32)GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+ (*way)->dpth = (int32)GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+ (*way)->dst = (int32)GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ for(i=0;i<2;++i) (*way)->state[i] = *p++;
+ for(i=0;i<2;++i) (*way)->cc[i] = *p++;
+
+ q = (UC *) (*way)->ident;
+ while((*q++ = *p++));
+
+ q = (UC *) (*way)->cmnt;
+ while((*q++ = *p++));
+
+ q = (UC *) (*way)->facility;
+ while((*q++ = *p++));
+
+ q = (UC *) (*way)->city;
+ while((*q++ = *p++));
+
+ q = (UC *) (*way)->addr;
+ while((*q++ = *p++));
+
+ q = (UC *) (*way)->cross_road;
+ while((*q++ = *p++));
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D150_Get ********************************************
+**
+** Get waypoint data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D150_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ int32 i;
+
+ p=s;
+
+ (*way)->prot = 150;
+ for(i=0;i<6;++i) (*way)->ident[i] = *p++;
+ for(i=0;i<2;++i) (*way)->cc[i] = *p++;
+ (*way)->wpt_class = *p++;
+
+ (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->alt = GPS_Util_Get_Short(p);
+ p+=sizeof(int16);
+
+ for(i=0;i<24;++i) (*way)->city[i] = *p++;
+ for(i=0;i<2;++i) (*way)->state[i] = *p++;
+ for(i=0;i<30;++i) (*way)->name[i] = *p++;
+ for(i=0;i<40;++i) (*way)->cmnt[i] = *p++;
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D151_Get *********************************************
+**
+** Get waypoint data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D151_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ int32 i;
+
+ p=s;
+
+ (*way)->prot = 151;
+ for(i=0;i<6;++i) (*way)->ident[i] = *p++;
+
+ (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ p+=sizeof(int32);
+
+ for(i=0;i<40;++i) (*way)->cmnt[i] = *p++;
+
+ (*way)->dst = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ for(i=0;i<30;++i) (*way)->name[i] = *p++;
+ for(i=0;i<24;++i) (*way)->city[i] = *p++;
+ for(i=0;i<2;++i) (*way)->state[i] = *p++;
+
+ (*way)->alt = GPS_Util_Get_Short(p);
+ p+=sizeof(int16);
+
+ for(i=0;i<2;++i) (*way)->cc[i] = *p++;
+
+ ++p;
+
+ (*way)->wpt_class = *p;
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D152_Get ********************************************
+**
+** Get waypoint data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D152_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ int32 i;
+
+ p=s;
+
+ (*way)->prot = 152;
+ for(i=0;i<6;++i) (*way)->ident[i] = *p++;
+
+ (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ p+=sizeof(int32);
+
+ for(i=0;i<40;++i) (*way)->cmnt[i] = *p++;
+
+ (*way)->dst = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ for(i=0;i<30;++i) (*way)->name[i] = *p++;
+ for(i=0;i<24;++i) (*way)->city[i] = *p++;
+ for(i=0;i<2;++i) (*way)->state[i] = *p++;
+
+ (*way)->alt = GPS_Util_Get_Short(p);
+ p+=sizeof(int16);
+
+ for(i=0;i<2;++i) (*way)->cc[i] = *p++;
+
+ ++p;
+
+ (*way)->wpt_class = *p;
+
+ return;
+}
+
+
+/* @funcstatic GPS_D154_Get ********************************************
+**
+** Get waypoint data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D154_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ int32 i;
+
+ p=s;
+
+ (*way)->prot = 154;
+ for(i=0;i<6;++i) (*way)->ident[i] = *p++;
+
+ (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ p+=sizeof(int32);
+
+ for(i=0;i<40;++i) (*way)->cmnt[i] = *p++;
+
+ (*way)->dst = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ for(i=0;i<30;++i) (*way)->name[i] = *p++;
+ for(i=0;i<24;++i) (*way)->city[i] = *p++;
+ for(i=0;i<2;++i) (*way)->state[i] = *p++;
+
+ (*way)->alt = GPS_Util_Get_Short(p);
+ p+=sizeof(int16);
+
+ for(i=0;i<2;++i) (*way)->cc[i] = *p++;
+
+ ++p;
+
+ (*way)->wpt_class = *p++;
+
+ (*way)->smbl = GPS_Util_Get_Short(p);
+
+ return;
+}
+
+
+/* @funcstatic GPS_D155_Get *********************************************
+**
+** Get waypoint data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D155_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ int32 i;
+
+ p=s;
+
+ (*way)->prot = 155;
+ for(i=0;i<6;++i) (*way)->ident[i] = *p++;
+
+ (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ p+=sizeof(int32);
+
+ for(i=0;i<40;++i) (*way)->cmnt[i] = *p++;
+
+ (*way)->dst = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ for(i=0;i<30;++i) (*way)->name[i] = *p++;
+ for(i=0;i<24;++i) (*way)->city[i] = *p++;
+ for(i=0;i<2;++i) (*way)->state[i] = *p++;
+
+ (*way)->alt = GPS_Util_Get_Short(p);
+ p+=sizeof(int16);
+
+ for(i=0;i<2;++i) (*way)->cc[i] = *p++;
+
+ ++p;
+
+ (*way)->wpt_class = *p++;
+
+ (*way)->smbl = GPS_Util_Get_Short(p);
+ p+=sizeof(int16);
+
+ (*way)->dspl = *p;
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D100_Send *******************************************
+**
+** Form waypoint data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D100_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ int32 i;
+
+ p = data;
+
+ for(i=0;i<6;++i) *p++ = way->ident[i];
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat));
+ p+=sizeof(int32);
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon));
+ p+=sizeof(int32);
+ GPS_Util_Put_Uint(p,0);
+ p+=sizeof(int32);
+ for(i=0;i<40;++i) *p++ = way->cmnt[i];
+
+ *len = 58;
+
+ return;
+}
+
+
+/* @funcstatic GPS_D101_Send ********************************************
+**
+** Form waypoint data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D101_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ int32 i;
+
+ p = data;
+
+ for(i=0;i<6;++i) *p++ = way->ident[i];
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat));
+ p+=sizeof(int32);
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon));
+ p+=sizeof(int32);
+ GPS_Util_Put_Uint(p,0);
+ p+=sizeof(int32);
+ for(i=0;i<40;++i) *p++ = way->cmnt[i];
+
+ GPS_Util_Put_Float(p,way->dst);
+ p+= sizeof(float);
+
+ *p = way->smbl;
+
+ *len = 63;
+
+ return;
+}
+
+
+/* @funcstatic GPS_D102_Send ********************************************
+**
+** Form waypoint data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D102_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ int32 i;
+
+ p = data;
+
+ for(i=0;i<6;++i) *p++ = way->ident[i];
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat));
+ p+=sizeof(int32);
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon));
+ p+=sizeof(int32);
+ GPS_Util_Put_Uint(p,0);
+ p+=sizeof(int32);
+ for(i=0;i<40;++i) *p++ = way->cmnt[i];
+
+ GPS_Util_Put_Float(p,way->dst);
+ p+= sizeof(float);
+
+ GPS_Util_Put_Short(p,way->smbl);
+
+ *len = 64;
+
+ return;
+}
+
+
+/* @funcstatic GPS_D103_Send *******************************************
+**
+** Form waypoint data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D103_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ int32 i;
+
+ p = data;
+
+ for(i=0;i<6;++i) *p++ = way->ident[i];
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat));
+ p+=sizeof(int32);
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon));
+ p+=sizeof(int32);
+ GPS_Util_Put_Uint(p,0);
+ p+=sizeof(int32);
+ for(i=0;i<40;++i) *p++ = way->cmnt[i];
+
+ *p++ = way->smbl;
+ *p = way->dspl;
+
+ *len = 60;
+
+ return;
+}
+
+
+/* @funcstatic GPS_D104_Send ********************************************
+**
+** Form waypoint data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D104_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ int32 i;
+
+ p = data;
+
+ for(i=0;i<6;++i) *p++ = way->ident[i];
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat));
+ p+=sizeof(int32);
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon));
+ p+=sizeof(int32);
+ GPS_Util_Put_Uint(p,0);
+ p+=sizeof(int32);
+ for(i=0;i<40;++i) *p++ = way->cmnt[i];
+
+ GPS_Util_Put_Float(p,way->dst);
+ p+= sizeof(float);
+
+ GPS_Util_Put_Short(p,way->smbl);
+ p+=sizeof(int16);
+
+ *p = way->dspl;
+
+ *len = 65;
+
+ return;
+}
+
+
+/* @funcstatic GPS_D105_Send *******************************************
+**
+** Form waypoint data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D105_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ UC *q;
+
+ p = data;
+
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat));
+ p+=sizeof(int32);
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon));
+ p+=sizeof(int32);
+
+ GPS_Util_Put_Short(p,way->smbl);
+ p+=sizeof(int16);
+
+ q = (UC *) way->wpt_ident;
+ while((*p++ = *q++));
+
+
+ *len = p-data;
+
+ return;
+}
+
+
+/* @funcstatic GPS_D106_Send ********************************************
+**
+** Form waypoint data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D106_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ UC *q;
+ int32 i;
+
+ p = data;
+
+ *p++ = way->wpt_class;
+ for(i=0;i<13;++i) *p++ = way->subclass[i];
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat));
+ p+=sizeof(int32);
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon));
+ p+=sizeof(int32);
+
+ GPS_Util_Put_Short(p,way->smbl);
+ p+=sizeof(int16);
+
+ q = (UC *) way->wpt_ident;
+ while((*p++ = *q++));
+ q = (UC *) way->lnk_ident;
+ while((*p++ = *q++));
+
+ *len = p-data;
+
+ return;
+}
+
+
+/* @funcstatic GPS_D107_Send ********************************************
+**
+** Form waypoint data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D107_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ int32 i;
+
+ p = data;
+ for(i=0;i<6;++i) *p++ = way->ident[i];
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat));
+ p+=sizeof(int32);
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon));
+ p+=sizeof(int32);
+ GPS_Util_Put_Uint(p,0);
+ p+=sizeof(int32);
+ for(i=0;i<40;++i) *p++ = way->cmnt[i];
+
+ *p++ = way->smbl;
+ *p++ = way->dspl;
+
+ GPS_Util_Put_Float(p,way->dst);
+ p+= sizeof(float);
+
+ *p = way->colour;
+
+ *len = 65;
+
+ return;
+}
+
+
+
+
+/* @funcstatic GPS_D108_Send ********************************************
+**
+** Form waypoint data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D108_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ UC *q;
+
+ int32 i;
+
+ p = data;
+
+ *p++ = way->wpt_class;
+ *p++ = way->colour;
+ *p++ = way->dspl;
+ *p++ = 0x60;
+ GPS_Util_Put_Short(p,way->smbl);
+ p+=sizeof(int16);
+ for(i=0;i<18;++i) *p++ = way->subclass[i];
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat));
+ p+=sizeof(int32);
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon));
+ p+=sizeof(int32);
+
+ GPS_Util_Put_Float(p,way->alt);
+ p+=sizeof(float);
+ GPS_Util_Put_Float(p,way->dpth);
+ p+=sizeof(float);
+ GPS_Util_Put_Float(p,way->dst);
+ p+=sizeof(float);
+
+ for(i=0;i<2;++i) *p++ = way->state[i];
+ for(i=0;i<2;++i) *p++ = way->cc[i];
+
+
+ q = (UC *) way->ident;
+ while((*p++ = *q++));
+ q = (UC *) way->cmnt;
+ while((*p++ = *q++));
+ q = (UC *) way->facility;
+ while((*p++ = *q++));
+ q = (UC *) way->city;
+ while((*p++ = *q++));
+ q = (UC *) way->addr;
+ while((*p++ = *q++));
+ q = (UC *) way->cross_road;
+ while((*p++ = *q++));
+
+ *len = p-data;
+
+ return;
+}
+
+
+
+
+/* @funcstatic GPS_D150_Send ********************************************
+**
+** Form waypoint data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D150_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ int32 i;
+
+ p = data;
+
+ for(i=0;i<6;++i) *p++ = way->ident[i];
+ for(i=0;i<2;++i) *p++ = way->cc[i];
+
+ if(way->wpt_class == 7) way->wpt_class = 0;
+ *p++ = way->wpt_class;
+
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat));
+ p+=sizeof(int32);
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon));
+ p+=sizeof(int32);
+
+ GPS_Util_Put_Short(p,way->alt);
+ p+=sizeof(int16);
+
+ for(i=0;i<24;++i) *p++ = way->city[i];
+ for(i=0;i<2;++i) *p++ = way->state[i];
+ for(i=0;i<30;++i) *p++ = way->name[i];
+ for(i=0;i<40;++i) *p++ = way->cmnt[i];
+
+ *len = 115;
+
+ return;
+}
+
+
+/* @funcstatic GPS_D151_Send ********************************************
+**
+** Form waypoint data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D151_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ int32 i;
+
+ p = data;
+
+ for(i=0;i<6;++i) *p++ = way->ident[i];
+
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat));
+ p+=sizeof(int32);
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon));
+ p+=sizeof(int32);
+ GPS_Util_Put_Uint(p,0);
+ p+=sizeof(int32);
+ for(i=0;i<40;++i) *p++ = way->cmnt[i];
+ GPS_Util_Put_Float(p,way->dst);
+ p+=sizeof(float);
+
+ for(i=0;i<30;++i) *p++ = way->name[i];
+ for(i=0;i<24;++i) *p++ = way->city[i];
+ for(i=0;i<2;++i) *p++ = way->state[i];
+
+ GPS_Util_Put_Short(p,way->alt);
+ p+=sizeof(int16);
+
+ for(i=0;i<2;++i) *p++ = way->cc[i];
+ *p++ = 0;
+
+ if(way->wpt_class == 3) way->wpt_class = 0;
+ *p = way->wpt_class;
+
+ *len = 124;
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D152_Send ********************************************
+**
+** Form waypoint data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D152_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ int32 i;
+
+ p = data;
+
+ for(i=0;i<6;++i) *p++ = way->ident[i];
+
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat));
+ p+=sizeof(int32);
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon));
+ p+=sizeof(int32);
+ GPS_Util_Put_Uint(p,0);
+ p+=sizeof(int32);
+ for(i=0;i<40;++i) *p++ = way->cmnt[i];
+ GPS_Util_Put_Float(p,way->dst);
+ p+=sizeof(float);
+
+ for(i=0;i<30;++i) *p++ = way->name[i];
+ for(i=0;i<24;++i) *p++ = way->city[i];
+ for(i=0;i<2;++i) *p++ = way->state[i];
+
+ GPS_Util_Put_Short(p,way->alt);
+ p+=sizeof(int16);
+
+ for(i=0;i<2;++i) *p++ = way->cc[i];
+ *p++ = 0;
+
+ if(way->wpt_class == 5) way->wpt_class = 0;
+ *p = way->wpt_class;
+
+ *len = 124;
+
+ return;
+}
+
+
+/* @funcstatic GPS_D154_Send *******************************************
+**
+** Form waypoint data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D154_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ int32 i;
+
+ p = data;
+
+ for(i=0;i<6;++i) *p++ = way->ident[i];
+
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat));
+ p+=sizeof(int32);
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon));
+ p+=sizeof(int32);
+ GPS_Util_Put_Uint(p,0);
+ p+=sizeof(int32);
+ for(i=0;i<40;++i) *p++ = way->cmnt[i];
+ GPS_Util_Put_Float(p,way->dst);
+ p+=sizeof(float);
+
+ for(i=0;i<30;++i) *p++ = way->name[i];
+ for(i=0;i<24;++i) *p++ = way->city[i];
+ for(i=0;i<2;++i) *p++ = way->state[i];
+
+ GPS_Util_Put_Short(p,way->alt);
+ p+=sizeof(int16);
+
+ for(i=0;i<2;++i) *p++ = way->cc[i];
+ *p++ = 0;
+
+ if(way->wpt_class == 9) way->wpt_class = 0;
+ *p++ = way->wpt_class;
+
+ GPS_Util_Put_Short(p,(int16)way->smbl);
+
+ *len = 126;
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D155_Send *******************************************
+**
+** Form waypoint data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D155_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ int32 i;
+
+ p = data;
+
+ for(i=0;i<6;++i) *p++ = way->ident[i];
+
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat));
+ p+=sizeof(int32);
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon));
+ p+=sizeof(int32);
+ GPS_Util_Put_Uint(p,0);
+ p+=sizeof(int32);
+ for(i=0;i<40;++i) *p++ = way->cmnt[i];
+ GPS_Util_Put_Float(p,way->dst);
+ p+=sizeof(float);
+
+ for(i=0;i<30;++i) *p++ = way->name[i];
+ for(i=0;i<24;++i) *p++ = way->city[i];
+ for(i=0;i<2;++i) *p++ = way->state[i];
+
+ GPS_Util_Put_Short(p,way->alt);
+ p+=sizeof(int16);
+
+ for(i=0;i<2;++i) *p++ = way->cc[i];
+ *p++ = 0;
+
+ if(way->wpt_class == 5) way->wpt_class = 0;
+ *p++ = way->wpt_class;
+
+ GPS_Util_Put_Short(p,(int16)way->smbl);
+ p+=sizeof(int16);
+
+ *p = way->dspl;
+
+ *len = 127;
+
+ return;
+}
+
+
+
+/* @func GPS_A200_Get ******************************************************
+**
+** Get route data from GPS
+**
+** @param [r] port [const char *] serial port
+** @param [w] way [GPS_PWay **] waypoint array
+**
+** @return [int32] number of waypoint entries
+************************************************************************/
+int32 GPS_A200_Get(const char *port, GPS_PWay **way)
+{
+ static UC data[2];
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 n;
+ int32 i;
+
+
+ if(!GPS_Serial_On(port,&fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+ GPS_Util_Put_Short(data,
+ COMMAND_ID[gps_device_command].Cmnd_Transfer_Rte);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ n = GPS_Util_Get_Short(rec->data);
+
+ if(n)
+ if(!((*way)=(GPS_PWay *)malloc(n*sizeof(GPS_PWay))))
+ {
+ GPS_Error("A200_Get: Insufficient memory");
+ return MEMORY_ERROR;
+ }
+
+
+ for(i=0;i<n;++i)
+ {
+ if(!((*way)[i]=GPS_Way_New()))
+ return MEMORY_ERROR;
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ if(rec->type == LINK_ID[gps_link_type].Pid_Rte_Hdr)
+ {
+ switch(gps_rte_hdr_type)
+ {
+ case pD200:
+ GPS_D200_Get(&((*way)[i]),rec->data);
+ break;
+ case pD201:
+ GPS_D201_Get(&((*way)[i]),rec->data);
+ break;
+ case pD202:
+ GPS_D202_Get(&((*way)[i]),rec->data);
+ break;
+ default:
+ GPS_Error("A200_GET: Unknown route protocol");
+ return PROTOCOL_ERROR;
+ }
+ continue;
+ }
+
+ if(rec->type != LINK_ID[gps_link_type].Pid_Rte_Wpt_Data)
+ {
+ GPS_Error("A200_GET: Non Pid_rte_Wpt_Data");
+ return FRAMING_ERROR;
+ }
+
+ (*way)[i]->isrte = 0;
+ (*way)[i]->islink = 0;
+
+ switch(gps_rte_type)
+ {
+ case pD100:
+ GPS_D100_Get(&((*way)[i]),rec->data);
+ break;
+ case pD101:
+ GPS_D101_Get(&((*way)[i]),rec->data);
+ break;
+ case pD102:
+ GPS_D102_Get(&((*way)[i]),rec->data);
+ break;
+ case pD103:
+ GPS_D103_Get(&((*way)[i]),rec->data);
+ break;
+ case pD104:
+ GPS_D104_Get(&((*way)[i]),rec->data);
+ break;
+ case pD105:
+ GPS_D105_Get(&((*way)[i]),rec->data);
+ break;
+ case pD106:
+ GPS_D106_Get(&((*way)[i]),rec->data);
+ break;
+ case pD107:
+ GPS_D107_Get(&((*way)[i]),rec->data);
+ break;
+ case pD108:
+ GPS_D108_Get(&((*way)[i]),rec->data);
+ break;
+ case pD150:
+ GPS_D150_Get(&((*way)[i]),rec->data);
+ break;
+ case pD151:
+ GPS_D151_Get(&((*way)[i]),rec->data);
+ break;
+ case pD152:
+ GPS_D152_Get(&((*way)[i]),rec->data);
+ break;
+ case pD154:
+ GPS_D154_Get(&((*way)[i]),rec->data);
+ break;
+ case pD155:
+ GPS_D155_Get(&((*way)[i]),rec->data);
+ break;
+ default:
+ GPS_Error("A200_GET: Unknown route protocol");
+ return PROTOCOL_ERROR;
+ }
+ (*way)[i-1]->prot = (*way)[i]->prot;
+ }
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt)
+ {
+ GPS_Error("A200_GET: Error transferring routes");
+ return FRAMING_ERROR;
+ }
+
+ if(i != n)
+ {
+ GPS_Error("A200_GET: Route entry number mismatch");
+ return FRAMING_ERROR;
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return n;
+}
+
+
+
+/* @func GPS_A201_Get ******************************************************
+**
+** Get route data from GPS
+**
+** @param [r] port [const char *] serial port
+** @param [w] way [GPS_PWay **] waypoint array
+**
+** @return [int32] number of waypoint entries
+************************************************************************/
+int32 GPS_A201_Get(const char *port, GPS_PWay **way)
+{
+ static UC data[2];
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 n;
+ int32 i;
+
+
+ if(!GPS_Serial_On(port,&fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+ GPS_Util_Put_Short(data,
+ COMMAND_ID[gps_device_command].Cmnd_Transfer_Rte);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ n = GPS_Util_Get_Short(rec->data);
+
+ if(n)
+ if(!((*way)=(GPS_PWay *)malloc(n*sizeof(GPS_PWay))))
+ {
+ GPS_Error("A201_Get: Insufficient memory");
+ return MEMORY_ERROR;
+ }
+
+
+ for(i=0;i<n;++i)
+ {
+ if(!((*way)[i]=GPS_Way_New()))
+ return MEMORY_ERROR;
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ if(rec->type == LINK_ID[gps_link_type].Pid_Rte_Hdr)
+ {
+ switch(gps_rte_hdr_type)
+ {
+ case pD200:
+ GPS_D200_Get(&((*way)[i]),rec->data);
+ break;
+ case pD201:
+ GPS_D201_Get(&((*way)[i]),rec->data);
+ break;
+ case pD202:
+ GPS_D202_Get(&((*way)[i]),rec->data);
+ break;
+ default:
+ GPS_Error("A201_GET: Unknown route protocol");
+ return PROTOCOL_ERROR;
+ }
+ (*way)[i]->islink = 0;
+ continue;
+ }
+
+
+ if(rec->type == LINK_ID[gps_link_type].Pid_Rte_Link_Data)
+ {
+ switch(gps_rte_link_type)
+ {
+ case pD210:
+ GPS_D210_Get(&((*way)[i]),rec->data);
+ break;
+ default:
+ GPS_Error("A201_GET: Unknown route protocol");
+ return PROTOCOL_ERROR;
+ }
+ (*way)[i]->isrte = 0;
+ (*way)[i]->islink = 1;
+ continue;
+ }
+
+ if(rec->type != LINK_ID[gps_link_type].Pid_Rte_Wpt_Data)
+ {
+ GPS_Error("A200_GET: Non Pid_rte_Wpt_Data");
+ return FRAMING_ERROR;
+ }
+
+ (*way)[i]->isrte = 0;
+ (*way)[i]->islink = 0;
+
+ switch(gps_rte_type)
+ {
+ case pD100:
+ GPS_D100_Get(&((*way)[i]),rec->data);
+ break;
+ case pD101:
+ GPS_D101_Get(&((*way)[i]),rec->data);
+ break;
+ case pD102:
+ GPS_D102_Get(&((*way)[i]),rec->data);
+ break;
+ case pD103:
+ GPS_D103_Get(&((*way)[i]),rec->data);
+ break;
+ case pD104:
+ GPS_D104_Get(&((*way)[i]),rec->data);
+ break;
+ case pD105:
+ GPS_D105_Get(&((*way)[i]),rec->data);
+ break;
+ case pD106:
+ GPS_D106_Get(&((*way)[i]),rec->data);
+ break;
+ case pD107:
+ GPS_D107_Get(&((*way)[i]),rec->data);
+ break;
+ case pD108:
+ GPS_D108_Get(&((*way)[i]),rec->data);
+ break;
+ case pD150:
+ GPS_D150_Get(&((*way)[i]),rec->data);
+ break;
+ case pD151:
+ GPS_D151_Get(&((*way)[i]),rec->data);
+ break;
+ case pD152:
+ GPS_D152_Get(&((*way)[i]),rec->data);
+ break;
+ case pD154:
+ GPS_D154_Get(&((*way)[i]),rec->data);
+ break;
+ case pD155:
+ GPS_D155_Get(&((*way)[i]),rec->data);
+ break;
+ default:
+ GPS_Error("A200_GET: Unknown route protocol");
+ return PROTOCOL_ERROR;
+ }
+ (*way)[i-1]->prot = (*way)[i]->prot;
+ }
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt)
+ {
+ GPS_Error("A200_GET: Error transferring routes");
+ return FRAMING_ERROR;
+ }
+
+ if(i != n)
+ {
+ GPS_Error("A200_GET: Route entry number mismatch");
+ return FRAMING_ERROR;
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return n;
+}
+
+
+
+/* @func GPS_A200_Send **************************************************
+**
+** Send routes to GPS
+**
+** @param [r] port [const char *] serial port
+** @param [r] trk [GPS_PWay *] waypoint array
+** @param [r] n [int32] number of waypoint entries
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_A200_Send(const char *port, GPS_PWay *way, int32 n)
+{
+ UC data[GPS_ARB_LEN];
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 i;
+ int32 len;
+ UC method;
+
+ if(!GPS_Serial_On(port,&fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+ GPS_Util_Put_Short(data,n);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A200_Send: Route start data not acknowledged");
+ return FRAMING_ERROR;
+ }
+
+
+ for(i=0;i<n;++i)
+ {
+ if(way[i]->isrte)
+ {
+ method = LINK_ID[gps_link_type].Pid_Rte_Hdr;
+
+ switch(gps_rte_hdr_type)
+ {
+ case pD200:
+ GPS_D200_Send(data,way[i],&len);
+ break;
+ case pD201:
+ GPS_D201_Send(data,way[i],&len);
+ break;
+ case pD202:
+ GPS_D202_Send(data,way[i],&len);
+ break;
+ default:
+ GPS_Error("A200_Send: Unknown route protocol");
+ return PROTOCOL_ERROR;
+ }
+ }
+ else
+ {
+ method = LINK_ID[gps_link_type].Pid_Rte_Wpt_Data;
+
+ switch(gps_rte_type)
+ {
+ case pD100:
+ GPS_D100_Send(data,way[i],&len);
+ break;
+ case pD101:
+ GPS_D101_Send(data,way[i],&len);
+ break;
+ case pD102:
+ GPS_D102_Send(data,way[i],&len);
+ break;
+ case pD103:
+ GPS_D103_Send(data,way[i],&len);
+ break;
+ case pD104:
+ GPS_D104_Send(data,way[i],&len);
+ break;
+ case pD105:
+ GPS_D105_Send(data,way[i],&len);
+ break;
+ case pD106:
+ GPS_D106_Send(data,way[i],&len);
+ break;
+ case pD107:
+ GPS_D107_Send(data,way[i],&len);
+ break;
+ case pD108:
+ GPS_D108_Send(data,way[i],&len);
+ break;
+ case pD150:
+ GPS_D150_Send(data,way[i],&len);
+ break;
+ case pD151:
+ GPS_D151_Send(data,way[i],&len);
+ break;
+ case pD152:
+ GPS_D152_Send(data,way[i],&len);
+ break;
+ case pD154:
+ GPS_D154_Send(data,way[i],&len);
+ break;
+ case pD155:
+ GPS_D155_Send(data,way[i],&len);
+ break;
+ default:
+ GPS_Error("A200_Send: Unknown route protocol");
+ return PROTOCOL_ERROR;
+ }
+ }
+
+
+ GPS_Make_Packet(&tra, method, data,len);
+
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A200_Send: Route packet not acknowledged");
+ return FRAMING_ERROR;
+ }
+ }
+
+ GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Wpt);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A200_Send: Route complete data not acknowledged");
+ return FRAMING_ERROR;
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return 1;
+}
+
+
+
+/* @func GPS_A201_Send **************************************************
+**
+** Send routes to GPS
+**
+** @param [r] port [const char *] serial port
+** @param [r] trk [GPS_PWay *] waypoint array
+** @param [r] n [int32] number of waypoint entries
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_A201_Send(const char *port, GPS_PWay *way, int32 n)
+{
+ UC data[GPS_ARB_LEN];
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 i;
+ int32 len;
+ UC method;
+
+ if(!GPS_Serial_On(port,&fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+ GPS_Util_Put_Short(data,n);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A200_Send: Route start data not acknowledged");
+ return FRAMING_ERROR;
+ }
+
+
+ for(i=0;i<n;++i)
+ {
+ if(way[i]->isrte)
+ {
+ method = LINK_ID[gps_link_type].Pid_Rte_Hdr;
+
+ switch(gps_rte_hdr_type)
+ {
+ case pD200:
+ GPS_D200_Send(data,way[i],&len);
+ break;
+ case pD201:
+ GPS_D201_Send(data,way[i],&len);
+ break;
+ case pD202:
+ GPS_D202_Send(data,way[i],&len);
+ break;
+ default:
+ GPS_Error("A200_Send: Unknown route protocol");
+ return PROTOCOL_ERROR;
+ }
+ }
+ else if(way[i]->islink)
+ {
+ method = LINK_ID[gps_link_type].Pid_Rte_Link_Data;
+
+ switch(gps_rte_link_type)
+ {
+ case pD210:
+ GPS_D210_Send(data,way[i],&len);
+ break;
+ default:
+ GPS_Error("A201_Send: Unknown route protocol");
+ return PROTOCOL_ERROR;
+ }
+ }
+ else
+ {
+ method = LINK_ID[gps_link_type].Pid_Rte_Wpt_Data;
+
+ switch(gps_rte_type)
+ {
+ case pD100:
+ GPS_D100_Send(data,way[i],&len);
+ break;
+ case pD101:
+ GPS_D101_Send(data,way[i],&len);
+ break;
+ case pD102:
+ GPS_D102_Send(data,way[i],&len);
+ break;
+ case pD103:
+ GPS_D103_Send(data,way[i],&len);
+ break;
+ case pD104:
+ GPS_D104_Send(data,way[i],&len);
+ break;
+ case pD105:
+ GPS_D105_Send(data,way[i],&len);
+ break;
+ case pD106:
+ GPS_D106_Send(data,way[i],&len);
+ break;
+ case pD107:
+ GPS_D107_Send(data,way[i],&len);
+ break;
+ case pD108:
+ GPS_D108_Send(data,way[i],&len);
+ break;
+ case pD150:
+ GPS_D150_Send(data,way[i],&len);
+ break;
+ case pD151:
+ GPS_D151_Send(data,way[i],&len);
+ break;
+ case pD152:
+ GPS_D152_Send(data,way[i],&len);
+ break;
+ case pD154:
+ GPS_D154_Send(data,way[i],&len);
+ break;
+ case pD155:
+ GPS_D155_Send(data,way[i],&len);
+ break;
+ default:
+ GPS_Error("A200_Send: Unknown route protocol");
+ return PROTOCOL_ERROR;
+ }
+ }
+
+
+ GPS_Make_Packet(&tra, method, data,len);
+
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A200_Send: Route packet not acknowledged");
+ return FRAMING_ERROR;
+ }
+ }
+
+ GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Wpt);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A200_Send: Route complete data not acknowledged");
+ return FRAMING_ERROR;
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return 1;
+}
+
+
+
+
+
+/* @funcstatic GPS_D200_Get ********************************************
+**
+** Get route header data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D200_Get(GPS_PWay *way, UC *s)
+{
+ (*way)->rte_prot = 200;
+ (*way)->rte_num = *s;
+ (*way)->isrte = 1;
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D201_Get *******************************************
+**
+** Get route header data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D201_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ int32 i;
+
+ p=s;
+
+ (*way)->rte_prot = 201;
+ (*way)->rte_num = *p++;
+ (*way)->isrte = 1;
+ for(i=0;i<20;++i) (*way)->rte_cmnt[i] = *p++;
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D202_Get ********************************************
+**
+** Get route header data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D202_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ UC *q;
+
+ p=s;
+
+ (*way)->rte_prot = 201;
+ (*way)->rte_num = *p++;
+ (*way)->isrte = 1;
+ q = (UC *) (*way)->rte_ident;
+ while((*q++=*p++));
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D210_Get ********************************************
+**
+** Get route link data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D210_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ UC *q;
+ int32 i;
+
+ p=s;
+
+ (*way)->rte_link_class = GPS_Util_Get_Short(p);
+ p+=sizeof(int16);
+ for(i=0;i<18;++i) (*way)->rte_link_subclass[i] = *p++;
+ q = (UC *) (*way)->rte_link_ident;
+ while((*q++=*p++));
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D200_Send *******************************************
+**
+** Form route header data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D200_Send(UC *data, GPS_PWay way, int32 *len)
+{
+
+ *data = way->rte_num;
+ *len = 1;
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D201_Send *******************************************
+**
+** Form route header data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D201_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ int32 i;
+
+ p = data;
+
+ *p++ = way->rte_num;
+ for(i=0;i<20;++i) *p++ = way->rte_cmnt[i];
+ *len = 21;
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D202_Send ********************************************
+**
+** Form route header data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D202_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ UC *q;
+
+ p = data;
+ q = (UC *) way->rte_ident;
+
+ while((*p++ = *q++));
+
+ *len = p-data;
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D210_Send ********************************************
+**
+** Form route link data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D210_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ UC *q;
+ int32 i;
+
+ p = data;
+
+ GPS_Util_Put_Short(p,way->rte_link_class);
+ p+=sizeof(int16);
+ for(i=0;i<18;++i) *p++ = way->rte_link_subclass[i];
+
+ q = (UC *) way->rte_link_ident;
+ while((*p++ = *q++));
+
+ *len = p-data;
+
+ return;
+}
+
+
+
+/* @func GPS_A300_Get ******************************************************
+**
+** Get track data from GPS
+**
+** @param [r] port [const char *] serial port
+** @param [w] trk [GPS_PTrack **] track array
+**
+** @return [int32] number of track entries
+************************************************************************/
+int32 GPS_A300_Get(const char *port, GPS_PTrack **trk)
+{
+ static UC data[2];
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 n;
+ int32 i;
+ int32 ret;
+
+
+ if(gps_trk_transfer == -1)
+ return GPS_UNSUPPORTED;
+
+ /* Only those GPS' with L001 can send track data */
+ if(!LINK_ID[gps_link_type].Pid_Trk_Data)
+ {
+ GPS_Warning("A300 protocol unsupported");
+ return GPS_UNSUPPORTED;
+ }
+
+ if(!GPS_Serial_On(port, &fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+ GPS_Util_Put_Short(data,
+ COMMAND_ID[gps_device_command].Cmnd_Transfer_Trk);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ return gps_errno;
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+
+ n = GPS_Util_Get_Short(rec->data);
+
+ if(n)
+ if(!((*trk)=(GPS_PTrack *)malloc(n*sizeof(GPS_PTrack))))
+ {
+ GPS_Error("A300_Get: Insufficient memory");
+ return MEMORY_ERROR;
+ }
+ for(i=0;i<n;++i)
+ if(!((*trk)[i]=GPS_Track_New()))
+ return MEMORY_ERROR;
+
+
+ switch(gps_trk_type)
+ {
+ case pD300:
+ ret = GPS_D300_Get(*trk,n,fd);
+ if(ret<0) return ret;
+ break;
+ default:
+ GPS_Error("A300_GET: Unknown track protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ if(ret != n)
+ {
+ GPS_Error("A300_GET: Track entry number mismatch");
+ return FRAMING_ERROR;
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return ret;
+}
+
+
+
+
+/* @func GPS_A301_Get ******************************************************
+**
+** Get track data from GPS
+**
+** @param [r] port [const char *] serial port
+** @param [w] trk [GPS_PTrack **] track array
+**
+** @return [int32] number of track entries
+************************************************************************/
+int32 GPS_A301_Get(const char *port, GPS_PTrack **trk)
+{
+ static UC data[2];
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 n;
+ int32 i;
+
+ if(gps_trk_transfer == -1)
+ return GPS_UNSUPPORTED;
+
+ /* Only those GPS' with L001 can send track data */
+ if(!LINK_ID[gps_link_type].Pid_Trk_Data)
+ {
+ GPS_Warning("A301 protocol unsupported");
+ return GPS_UNSUPPORTED;
+ }
+
+ if(!GPS_Serial_On(port, &fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+ GPS_Util_Put_Short(data,
+ COMMAND_ID[gps_device_command].Cmnd_Transfer_Trk);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ return gps_errno;
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+
+ n = GPS_Util_Get_Short(rec->data);
+
+ if(n)
+ if(!((*trk)=(GPS_PTrack *)malloc(n*sizeof(GPS_PTrack))))
+ {
+ GPS_Error("A301_Get: Insufficient memory");
+ return MEMORY_ERROR;
+ }
+ for(i=0;i<n;++i)
+ if(!((*trk)[i]=GPS_Track_New()))
+ return MEMORY_ERROR;
+
+
+ for(i=0;i<n;++i)
+ {
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ if(rec->type == LINK_ID[gps_link_type].Pid_Trk_Hdr)
+ {
+ switch(gps_trk_hdr_type)
+ {
+ case pD310:
+ GPS_D310_Get(&((*trk)[i]),rec->data);
+ break;
+ default:
+ GPS_Error("A301_Get: Unknown track protocol");
+ return PROTOCOL_ERROR;
+ }
+ (*trk)[i]->ishdr = 1;
+ continue;
+ }
+
+ if(rec->type != LINK_ID[gps_link_type].Pid_Trk_Data)
+ {
+ GPS_Error("A301_Get: Non-Pid_Trk_Data");
+ return FRAMING_ERROR;
+ }
+
+ (*trk)[i]->ishdr = 0;
+
+ switch(gps_trk_type)
+ {
+ case pD300:
+ GPS_D300b_Get(&((*trk)[i]),rec->data);
+ break;
+ case pD301:
+ GPS_D301b_Get(&((*trk)[i]),rec->data);
+ break;
+ default:
+ GPS_Error("A301_GET: Unknown track protocol");
+ return PROTOCOL_ERROR;
+ }
+ }
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt)
+ {
+ GPS_Error("A301_Get: Error transferring tracks");
+ return FRAMING_ERROR;
+ }
+
+ if(i != n)
+ {
+ GPS_Error("A301_GET: Track entry number mismatch");
+ return FRAMING_ERROR;
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return n;
+}
+
+
+
+
+
+/* @func GPS_A300_Send **************************************************
+**
+** Send track log to GPS
+**
+** @param [r] port [const char *] serial port
+** @param [r] trk [GPS_PTrack *] track array
+** @param [r] n [int32] number of track entries
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_A300_Send(const char *port, GPS_PTrack *trk, int32 n)
+{
+ UC data[GPS_ARB_LEN];
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 i;
+ int32 len;
+
+ if(gps_trk_transfer == -1)
+ return GPS_UNSUPPORTED;
+
+ /* Only those GPS' with L001 can send track data */
+ if(!LINK_ID[gps_link_type].Pid_Trk_Data)
+ {
+ GPS_Warning("A300 protocol unsupported");
+ return GPS_UNSUPPORTED;
+ }
+
+ if(!GPS_Serial_On(port, &fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+ GPS_Util_Put_Short(data,n);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A300_Send: Track start data not acknowledged");
+ return FRAMING_ERROR;
+ }
+
+ for(i=0;i<n;++i)
+ {
+ switch(gps_trk_type)
+ {
+ case pD300:
+ GPS_D300_Send(data,trk[i]);
+ len = 13;
+ break;
+ default:
+ GPS_Error("A300_Send: Unknown track protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Trk_Data,
+ data,len);
+
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A300_Send: Pid_Trk_Data not acknowledgedn");
+ return FRAMING_ERROR;
+ }
+ }
+
+ GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Trk);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A300_Send: Track complete data not acknowledged");
+ return FRAMING_ERROR;
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return 1;
+}
+
+
+
+/* @func GPS_A301_Send **************************************************
+**
+** Send track log to GPS
+**
+** @param [r] port [const char *] serial port
+** @param [r] trk [GPS_PTrack *] track array
+** @param [r] n [int32] number of track entries
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_A301_Send(const char *port, GPS_PTrack *trk, int32 n)
+{
+ UC data[GPS_ARB_LEN];
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 i;
+ int32 len;
+ UC method;
+
+ if(gps_trk_transfer == -1)
+ return GPS_UNSUPPORTED;
+
+ /* Only those GPS' with L001 can send track data */
+ if(!LINK_ID[gps_link_type].Pid_Trk_Data)
+ {
+ GPS_Warning("A301 protocol unsupported");
+ return GPS_UNSUPPORTED;
+ }
+
+ if(!GPS_Serial_On(port, &fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+ GPS_Util_Put_Short(data,n);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A301_Send: Track start data not acknowledged");
+ return FRAMING_ERROR;
+ }
+
+
+ for(i=0;i<n;++i)
+ {
+ if(trk[i]->ishdr)
+ {
+ method = LINK_ID[gps_link_type].Pid_Trk_Hdr;
+
+ switch(gps_trk_hdr_type)
+ {
+ case pD310:
+ GPS_D310_Send(data,trk[i],&len);
+ break;
+ default:
+ GPS_Error("A301_Send: Unknown track protocol");
+ return PROTOCOL_ERROR;
+ }
+ }
+ else
+ {
+ method = LINK_ID[gps_link_type].Pid_Trk_Data;
+
+ switch(gps_trk_type)
+ {
+ case pD300:
+ GPS_D300_Send(data,trk[i]);
+ len = 13;
+ break;
+ case pD301:
+ GPS_D301_Send(data,trk[i]);
+ len = 21;
+ break;
+ default:
+ GPS_Error("A301_Send: Unknown track protocol");
+ return PROTOCOL_ERROR;
+ }
+ }
+
+
+ GPS_Make_Packet(&tra, method, data,len);
+
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A301_Send: Track packet not acknowledgedn");
+ return FRAMING_ERROR;
+ }
+ }
+
+
+ GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Trk);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A301_Send: Track complete data not acknowledged");
+ return FRAMING_ERROR;
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return 1;
+}
+
+
+
+/* @func GPS_D300_Get ******************************************************
+**
+** Get track data
+**
+** @param [w] trk [GPS_PTrack *] track array
+** @param [r] entries [int32] number of packets to receive
+** @param [r] fd [int32] file descriptor
+**
+** @return [int32] number of entries read
+************************************************************************/
+int32 GPS_D300_Get(GPS_PTrack *trk, int32 entries, int32 fd)
+{
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 i;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+
+ for(i=0;i<entries;++i)
+ {
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ GPS_A300_Translate(rec->data, &trk[i]);
+ }
+
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+
+ if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt)
+ {
+ GPS_Error("D300_GET: Error transferring track log");
+ return FRAMING_ERROR;
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ return i;
+}
+
+
+
+/* @func GPS_D300b_Get ******************************************************
+**
+** Get track data (A301 protocol)
+**
+** @param [w] trk [GPS_PTrack *] track
+** @param [r] data [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+void GPS_D300b_Get(GPS_PTrack *trk, UC *data)
+{
+
+ GPS_A300_Translate(data, trk);
+ return;
+}
+
+
+
+/* @func GPS_D301b_Get ******************************************************
+**
+** Get track data (A301 protocol)
+**
+** @param [w] trk [GPS_PTrack *] track
+** @param [r] data [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+void GPS_D301b_Get(GPS_PTrack *trk, UC *data)
+{
+ UC *p;
+ uint32 t;
+
+ p=data;
+
+ (*trk)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*trk)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ t = GPS_Util_Get_Uint(p);
+ if(!t || t==0x7fffffff || t==0xffffffff)
+ (*trk)->Time=0;
+ else
+ (*trk)->Time = GPS_Math_Gtime_To_Utime((time_t)t);
+ p+=sizeof(uint32);
+
+ (*trk)->alt = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*trk)->dpth = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*trk)->tnew = *p;
+
+ return;
+}
+
+
+
+/* @func GPS_D310_Get ******************************************************
+**
+** Get track header data (A301 protocol)
+**
+** @param [w] trk [GPS_PTrack *] track
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+void GPS_D310_Get(GPS_PTrack *trk, UC *s)
+{
+ UC *p;
+ UC *q;
+
+ p=s;
+
+ (*trk)->dspl = *p++;
+ (*trk)->colour = *p++;
+
+ q = (UC *) (*trk)->trk_ident;
+
+ while((*q++ = *p++));
+
+ return;
+}
+
+
+
+/* @func GPS_D300_Send **************************************************
+**
+** Form track data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] trk [GPS_PTrack] track data
+**
+** @return [void]
+************************************************************************/
+void GPS_D300_Send(UC *data, GPS_PTrack trk)
+{
+ UC *p;
+
+ p = data;
+ GPS_A300_Encode(p,trk);
+
+ return;
+}
+
+
+
+/* @func GPS_D301_Send **************************************************
+**
+** Form track data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] trk [GPS_PTrack] track data
+**
+** @return [void]
+************************************************************************/
+void GPS_D301_Send(UC *data, GPS_PTrack trk)
+{
+ UC *p;
+
+ p = data;
+ GPS_A300_Encode(p,trk);
+ p = data+12;
+
+ GPS_Util_Put_Float(p,trk->alt);
+ p+=sizeof(float);
+ GPS_Util_Put_Float(p,trk->dpth);
+ p+=sizeof(float);
+
+ *p = trk->tnew;
+
+ return;
+}
+
+
+
+/* @func GPS_D310_Send **************************************************
+**
+** Form track header data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] trk [GPS_PTrack] track data
+** @param [w] len [int32 *] length of data
+**
+** @return [void]
+************************************************************************/
+void GPS_D310_Send(UC *data, GPS_PTrack trk, int32 *len)
+{
+ UC *p;
+ UC *q;
+
+ p = data;
+
+ *p++ = trk->dspl;
+ *p++ = trk->colour;
+
+ q = (UC *) trk->trk_ident;
+ while((*p++ = *q++));
+
+ *len = p-data;
+
+ return;
+}
+
+
+/* @funcstatic GPS_A300_Translate ***************************************
+**
+** Translate track packet to track structure
+**
+** @param [r] s [const UC *] track packet data
+** @param [w] trk [GPS_PTrack *] track entry pointer
+**
+** @return [void]
+************************************************************************/
+static void GPS_A300_Translate(UC *s, GPS_PTrack *trk)
+{
+ UC *p;
+ uint32 t;
+
+ p=s;
+
+ (*trk)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*trk)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ t = GPS_Util_Get_Uint(p);
+ if(!t || t==0x7fffffff || t==0xffffffff)
+ (*trk)->Time=0;
+ else
+ (*trk)->Time = GPS_Math_Gtime_To_Utime((time_t)t);
+ p+=sizeof(uint32);
+
+ (*trk)->tnew = *p;
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_A300_Encode ***************************************
+**
+** Encode track structure to track packet
+**
+** @param [w] s [UC *] string to write to
+** @param [r] trk [GPS_PTrack] track entry
+**
+** @return [void]
+************************************************************************/
+static void GPS_A300_Encode(UC *s, GPS_PTrack trk)
+{
+ UC *p;
+
+ p=s;
+
+ GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(trk->lat));
+ p+=sizeof(int32);
+
+ GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(trk->lon));
+ p+=sizeof(int32);
+
+ GPS_Util_Put_Uint(p,GPS_Math_Utime_To_Gtime(trk->Time));
+ p+=sizeof(uint32);
+
+ *p = (UC) trk->tnew;
+
+ return;
+}
+
+
+
+/* @func GPS_A400_Get **************************************************
+**
+** Get proximity waypoint data from GPS
+**
+** @param [r] port [const char *] serial port
+** @param [w] way [GPS_PWay **] waypoint array
+**
+** @return [int32] number of waypoint entries
+************************************************************************/
+int32 GPS_A400_Get(const char *port, GPS_PWay **way)
+{
+ static UC data[2];
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 n;
+ int32 i;
+
+ if(gps_prx_waypt_transfer == -1)
+ return GPS_UNSUPPORTED;
+
+
+ if(!GPS_Serial_On(port, &fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+
+ GPS_Util_Put_Short(data,
+ COMMAND_ID[gps_device_command].Cmnd_Transfer_Prx);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ if(!GPS_Serial_Chars_Ready(fd))
+ {
+ GPS_Warning("A400 (ppx) protocol not supported");
+ GPS_Packet_Del(&rec);
+ GPS_Packet_Del(&tra);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return GPS_UNSUPPORTED;
+ }
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ n = GPS_Util_Get_Short(rec->data);
+
+ if(n)
+ if(!((*way)=(GPS_PWay *)malloc(n*sizeof(GPS_PWay))))
+ {
+ GPS_Error("A400_Get: Insufficient memory");
+ return MEMORY_ERROR;
+ }
+
+
+ for(i=0;i<n;++i)
+ {
+ if(!((*way)[i]=GPS_Way_New()))
+ return MEMORY_ERROR;
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+
+ switch(gps_prx_waypt_type)
+ {
+ case pD400:
+ GPS_D400_Get(&((*way)[i]),rec->data);
+ break;
+ case pD101:
+ GPS_D101_Get(&((*way)[i]),rec->data);
+ break;
+ case pD102:
+ GPS_D102_Get(&((*way)[i]),rec->data);
+ break;
+ case pD403:
+ GPS_D403_Get(&((*way)[i]),rec->data);
+ break;
+ case pD104:
+ GPS_D104_Get(&((*way)[i]),rec->data);
+ break;
+ case pD105:
+ GPS_D105_Get(&((*way)[i]),rec->data);
+ break;
+ case pD106:
+ GPS_D106_Get(&((*way)[i]),rec->data);
+ break;
+ case pD107:
+ GPS_D107_Get(&((*way)[i]),rec->data);
+ break;
+ case pD108:
+ GPS_D108_Get(&((*way)[i]),rec->data);
+ break;
+ case pD450:
+ GPS_D450_Get(&((*way)[i]),rec->data);
+ break;
+ case pD151:
+ GPS_D151_Get(&((*way)[i]),rec->data);
+ break;
+ case pD152:
+ GPS_D152_Get(&((*way)[i]),rec->data);
+ break;
+ case pD154:
+ GPS_D154_Get(&((*way)[i]),rec->data);
+ break;
+ case pD155:
+ GPS_D155_Get(&((*way)[i]),rec->data);
+ break;
+ default:
+ GPS_Error("A400_GET: Unknown prx waypoint protocol");
+ return PROTOCOL_ERROR;
+ }
+ }
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt)
+ {
+ GPS_Error("A400_GET: Error transferring prx waypoints");
+ return FRAMING_ERROR;
+ }
+
+ if(i != n)
+ {
+ GPS_Error("A400_GET: Prx waypoint entry number mismatch");
+ return FRAMING_ERROR;
+ }
+
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return n;
+}
+
+
+
+/* @func GPS_A400_Send **************************************************
+**
+** Send proximity waypoints to GPS
+**
+** @param [r] port [const char *] serial port
+** @param [r] trk [GPS_PWay *] waypoint array
+** @param [r] n [int32] number of waypoint entries
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_A400_Send(const char *port, GPS_PWay *way, int32 n)
+{
+ UC data[GPS_ARB_LEN];
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 i;
+ int32 len;
+
+ if(gps_prx_waypt_transfer == -1)
+ return GPS_UNSUPPORTED;
+
+ if(!GPS_Serial_On(port, &fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+
+ GPS_Util_Put_Short(data,n);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A400_Send: Prx start data not acknowledgedn");
+ return FRAMING_ERROR;
+ }
+
+
+ for(i=0;i<n;++i)
+ {
+ switch(gps_prx_waypt_type)
+ {
+ case pD400:
+ GPS_D400_Send(data,way[i],&len);
+ break;
+ case pD101:
+ GPS_D101_Send(data,way[i],&len);
+ break;
+ case pD102:
+ GPS_D102_Send(data,way[i],&len);
+ break;
+ case pD403:
+ GPS_D403_Send(data,way[i],&len);
+ break;
+ case pD104:
+ GPS_D104_Send(data,way[i],&len);
+ break;
+ case pD105:
+ GPS_D105_Send(data,way[i],&len);
+ break;
+ case pD106:
+ GPS_D106_Send(data,way[i],&len);
+ break;
+ case pD107:
+ GPS_D107_Send(data,way[i],&len);
+ break;
+ case pD108:
+ GPS_D108_Send(data,way[i],&len);
+ break;
+ case pD450:
+ GPS_D450_Send(data,way[i],&len);
+ break;
+ case pD151:
+ GPS_D151_Send(data,way[i],&len);
+ break;
+ case pD152:
+ GPS_D152_Send(data,way[i],&len);
+ break;
+ case pD154:
+ GPS_D154_Send(data,way[i],&len);
+ break;
+ case pD155:
+ GPS_D155_Send(data,way[i],&len);
+ break;
+ default:
+ GPS_Error("A400_Send: Unknown prx waypoint protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Prx_Wpt_Data,
+ data,len);
+
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A400_Send: Pid_Prx_Wpt_Data not acknowledged");
+ return FRAMING_ERROR;
+ }
+ }
+
+ GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Prx);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A400_Send: Prx waypoint complete data not acknowledged");
+ return FRAMING_ERROR;
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return 1;
+}
+
+
+
+/* @funcstatic GPS_D400_Get ********************************************
+**
+** Get proximity waypoint data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D400_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ int32 i;
+
+ p=s;
+
+ (*way)->prot = 400;
+ for(i=0;i<6;++i) (*way)->ident[i] = *p++;
+
+ (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ p+=sizeof(int32);
+
+ for(i=0;i<40;++i) (*way)->cmnt[i] = *p++;
+
+ (*way)->dst=GPS_Util_Get_Float(p);
+
+
+ return;
+}
+
+
+/* @funcstatic GPS_D403_Get ********************************************
+**
+** Get proximity waypoint data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D403_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ int32 i;
+
+ p=s;
+
+ (*way)->prot = 403;
+ for(i=0;i<6;++i) (*way)->ident[i] = *p++;
+
+ (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ p+=sizeof(int32);
+
+ for(i=0;i<40;++i) (*way)->cmnt[i] = *p++;
+
+ (*way)->smbl = *p++;
+ (*way)->dspl = *p++;
+
+ (*way)->dst=GPS_Util_Get_Float(p);
+
+ return;
+}
+
+
+/* @funcstatic GPS_D450_Get ********************************************
+**
+** Get proximity waypoint data
+**
+** @param [w] way [GPS_PWay *] waypoint array
+** @param [r] s [UC *] packet data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D450_Get(GPS_PWay *way, UC *s)
+{
+ UC *p;
+ int32 i;
+
+ p=s;
+
+ (*way)->prot = 450;
+
+ (*way)->idx = GPS_Util_Get_Short(p);
+ p+=sizeof(int16);
+
+ for(i=0;i<6;++i) (*way)->ident[i] = *p++;
+ for(i=0;i<2;++i) (*way)->cc[i] = *p++;
+ (*way)->wpt_class = *p++;
+
+ (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
+ p+=sizeof(int32);
+
+ (*way)->alt = GPS_Util_Get_Short(p);
+ p+=sizeof(int16);
+
+ for(i=0;i<24;++i) (*way)->city[i] = *p++;
+ for(i=0;i<2;++i) (*way)->state[i] = *p++;
+ for(i=0;i<30;++i) (*way)->name[i] = *p++;
+ for(i=0;i<40;++i) (*way)->cmnt[i] = *p++;
+
+ (*way)->dst=GPS_Util_Get_Float(p);
+
+ return;
+}
+
+
+/* @funcstatic GPS_D400_Send ********************************************
+**
+** Form proximity waypoint data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D400_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ int32 i;
+
+ p = data;
+
+ for(i=0;i<6;++i) *p++ = way->ident[i];
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat));
+ p+=sizeof(int32);
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon));
+ p+=sizeof(int32);
+ GPS_Util_Put_Uint(p,0);
+ p+=sizeof(int32);
+ for(i=0;i<40;++i) *p++ = way->cmnt[i];
+
+ GPS_Util_Put_Float(p,way->dst);
+
+ *len = 62;
+
+ return;
+}
+
+
+/* @funcstatic GPS_D403_Send *******************************************
+**
+** Form proximity waypoint data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D403_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ int32 i;
+
+ p = data;
+
+ for(i=0;i<6;++i) *p++ = way->ident[i];
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat));
+ p+=sizeof(int32);
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon));
+ p+=sizeof(int32);
+ GPS_Util_Put_Uint(p,0);
+ p+=sizeof(int32);
+ for(i=0;i<40;++i) *p++ = way->cmnt[i];
+
+ *p++ = way->smbl;
+ *p = way->dspl;
+
+ GPS_Util_Put_Float(p,way->dst);
+
+ *len = 64;
+
+ return;
+}
+
+
+/* @funcstatic GPS_D450_Send *******************************************
+**
+** Form proximity waypoint data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] way [GPS_PWay] waypoint data
+** @param [w] len [int32 *] packet length
+**
+** @return [void]
+************************************************************************/
+static void GPS_D450_Send(UC *data, GPS_PWay way, int32 *len)
+{
+ UC *p;
+ int32 i;
+
+ p = data;
+
+ GPS_Util_Put_Short(p,way->idx);
+ p+=sizeof(int16);
+
+ for(i=0;i<6;++i) *p++ = way->ident[i];
+ for(i=0;i<2;++i) *p++ = way->cc[i];
+ *p++ = way->wpt_class;
+
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat));
+ p+=sizeof(int32);
+ GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon));
+ p+=sizeof(int32);
+
+ GPS_Util_Put_Short(p,way->alt);
+ p+=sizeof(int16);
+
+ for(i=0;i<24;++i) *p++ = way->city[i];
+ for(i=0;i<2;++i) *p++ = way->state[i];
+ for(i=0;i<30;++i) *p++ = way->name[i];
+ for(i=0;i<40;++i) *p++ = way->cmnt[i];
+
+ GPS_Util_Put_Float(p,way->dst);
+
+
+ *len = 121;
+
+ return;
+}
+
+
+
+/* @func GPS_A500_Get ******************************************************
+**
+** Get almanac from GPS
+**
+** @param [r] port [const char *] serial port
+** @param [w] alm [GPS_PAlmanac **] almanac array
+**
+** @return [int32] number of almanac entries
+************************************************************************/
+int32 GPS_A500_Get(const char *port, GPS_PAlmanac **alm)
+{
+ static UC data[2];
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 n;
+ int32 i;
+ int32 ret;
+
+ if(!GPS_Serial_On(port, &fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+
+ GPS_Util_Put_Short(data,
+ COMMAND_ID[gps_device_command].Cmnd_Transfer_Alm);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ n = GPS_Util_Get_Short(rec->data);
+
+ if(n)
+ if(!((*alm)=(GPS_PAlmanac *)malloc(n*sizeof(GPS_PAlmanac))))
+ {
+ GPS_Error("A500_Get: Insufficient memory");
+ return MEMORY_ERROR;
+ }
+ for(i=0;i<n;++i)
+ if(!((*alm)[i]=GPS_Almanac_New()))
+ return MEMORY_ERROR;
+
+
+ switch(gps_almanac_type)
+ {
+ case pD500:
+ ret = GPS_D500_Get(*alm,n,fd);
+ break;
+ case pD501:
+ ret = GPS_D501_Get(*alm,n,fd);
+ break;
+ case pD550:
+ ret = GPS_D550_Get(*alm,n,fd);
+ break;
+ case pD551:
+ ret = GPS_D551_Get(*alm,n,fd);
+ break;
+ default:
+ GPS_Error("A500_GET: Unknown almanac protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ if(ret < 0) return ret;
+ if(ret != n)
+ {
+ GPS_Error("A500_GET: Almanac entry number mismatch");
+ return FRAMING_ERROR;
+ }
+
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return ret;
+}
+
+
+
+
+
+/* @func GPS_A500_Send **************************************************
+**
+** Send almanac to GPS
+**
+** @param [r] port [const char *] serial port
+** @param [r] alm [GPS_PAlmanac *] almanac array
+** @param [r] n [int32] number of almanac entries
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_A500_Send(const char *port, GPS_PAlmanac *alm, int32 n)
+{
+ UC data[GPS_ARB_LEN];
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 i;
+ int32 len;
+ int32 timesent;
+ int32 posnsent;
+ int32 ret;
+
+ if(!GPS_Serial_On(port, &fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+
+ GPS_Util_Put_Short(data,n);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A500_Send: Almanac start data not acknowledged");
+ return FRAMING_ERROR;
+ }
+
+
+ for(i=0;i<n;++i)
+ {
+ switch(gps_almanac_type)
+ {
+ case pD500:
+ if(n!=32)
+ {
+ GPS_Error("A500_Send: SATELLITES: n!=32 specified");
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+ return PROTOCOL_ERROR;
+ }
+ GPS_D500_Send(data,alm[i]);
+ len = 42;
+ break;
+ case pD501:
+ if(n!=32)
+ {
+ GPS_Error("A500_Send: SATELLITES: n!=32 specified");
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+ return PROTOCOL_ERROR;
+ }
+ GPS_D501_Send(data,alm[i]);
+ len = 43;
+ break;
+ case pD550:
+ GPS_D550_Send(data,alm[i]);
+ len = 43;
+ break;
+ case pD551:
+ GPS_D551_Send(data,alm[i]);
+ len = 44;
+ break;
+ default:
+ GPS_Error("A500_Send: Unknown almanac protocol");
+ return 0;
+ }
+
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Almanac_Data,
+ data,len);
+
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A500_Send: Almanac Pid_Almanac_Data not acknowledged");
+ return FRAMING_ERROR;
+ }
+ }
+
+ GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Alm);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ {
+ GPS_Error("A500_Send: Almanac complete data not acknowledged");
+ return FRAMING_ERROR;
+ }
+
+ timesent=posnsent=0;
+
+ /*
+ * Allow GPS a little while to decide whether it wants to ask for
+ * the time. Note that the time sent is held in gps_save_time
+ * global
+ */
+ if(GPS_Serial_Wait(fd))
+ {
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ if(rec->type == LINK_ID[gps_link_type].Pid_Command_Data &&
+ GPS_Util_Get_Short(rec->data) == COMMAND_ID[gps_device_command].
+ Cmnd_Transfer_Time)
+ {
+ GPS_User("INFO: GPS time request. Sending....");
+ ret = GPS_Rqst_Send_Time(fd,gps_save_time);
+ if(ret < 0) return ret;
+ timesent=1;
+ }
+ }
+
+
+
+ /*
+ * Allow GPS a little while to decide whether it wants to ask for
+ * the position. Note that the posn sent is held in gps_save_lat
+ * and gps_save_lon global!
+ */
+ if(GPS_Serial_Wait(fd))
+ {
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ if(rec->type == LINK_ID[gps_link_type].Pid_Command_Data &&
+ GPS_Util_Get_Short(rec->data) == COMMAND_ID[gps_device_command].
+ Cmnd_Transfer_Posn)
+ {
+ GPS_User("INFO: GPS position request. Sending....");
+ ret = GPS_Rqst_Send_Position(fd,gps_save_lat,gps_save_lon);
+ if(ret < 0) return ret;
+ posnsent=1;
+ }
+ }
+
+ if(!timesent)
+ {
+ ret = GPS_Rqst_Send_Time(fd,gps_save_time);
+ if(ret < 0) return ret;
+ }
+
+
+ if(!posnsent)
+ {
+ ret = GPS_Rqst_Send_Position(fd,gps_save_lat,gps_save_lon);
+ if(ret < 0) return ret;
+ }
+
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return 1;
+}
+
+
+
+/* @funcstatic GPS_D500_Get ********************************************
+**
+** Get almanac data
+**
+** @param [w] alm [GPS_PAlmanac *] almanac array
+** @param [r] entries [int32] number of packets to receive
+** @param [r] fd [int32] file descriptor
+**
+** @return [int32] number of entries read
+************************************************************************/
+static int32 GPS_D500_Get(GPS_PAlmanac *alm, int32 entries, int32 fd)
+{
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 i;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+
+ for(i=0;i<entries;++i)
+ {
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ GPS_A500_Translate(rec->data, &alm[i]);
+ }
+
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+
+ if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt)
+ {
+ GPS_Error("D500_GET: Error transferring almanac");
+ return FRAMING_ERROR;
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ return i;
+}
+
+
+/* @funcstatic GPS_D501_Get ********************************************
+**
+** Get almanac data
+**
+** @param [w] alm [GPS_PAlmanac *] almanac array
+** @param [r] entries [int32] number of packets to receive
+** @param [r] fd [int32] file descriptor
+**
+** @return [int32] number of entries read
+************************************************************************/
+static int32 GPS_D501_Get(GPS_PAlmanac *alm, int32 entries, int32 fd)
+{
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 i;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+
+ for(i=0;i<entries;++i)
+ {
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ GPS_A500_Translate(rec->data, &alm[i]);
+ alm[i]->hlth=rec->data[42];
+ }
+
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+
+ if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt)
+ {
+ GPS_Error("D501_GET: Error transferring almanac");
+ return FRAMING_ERROR;
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ return i;
+}
+
+
+
+/* @funcstatic GPS_D550_Get *********************************************
+**
+** Get almanac data
+**
+** @param [w] alm [GPS_PAlmanac *] almanac array
+** @param [r] entries [int32] number of packets to receive
+** @param [r] fd [int32] file descriptor
+**
+** @return [int32] number of entries read
+************************************************************************/
+static int32 GPS_D550_Get(GPS_PAlmanac *alm, int32 entries, int32 fd)
+{
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 i;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+
+ for(i=0;i<entries;++i)
+ {
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ alm[i]->svid = rec->data[0];
+ GPS_A500_Translate(rec->data+1, &alm[i]);
+ }
+
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt)
+ {
+ GPS_Error("D550_GET: Error transferring almanac");
+ return FRAMING_ERROR;
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ return i;
+}
+
+
+
+/* @funcstatic GPS_D551_Get *********************************************
+**
+** Get almanac data
+**
+** @param [w] alm [GPS_PAlmanac *] almanac array
+** @param [r] entries [int32] number of packets to receive
+** @param [r] fd [int32] file descriptor
+**
+** @return [int32] number of entries read
+************************************************************************/
+static int32 GPS_D551_Get(GPS_PAlmanac *alm, int32 entries, int32 fd)
+{
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 i;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+
+ for(i=0;i<entries;++i)
+ {
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ alm[i]->svid = rec->data[0];
+ GPS_A500_Translate(rec->data+1, &alm[i]);
+ alm[i]->hlth = rec->data[43];
+ }
+
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+
+ if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt)
+ {
+ GPS_Error("D551_GET: Error transferring almanac\n");
+ return FRAMING_ERROR;
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ return i;
+}
+
+
+
+/* @funcstatic GPS_A500_Translate ***************************************
+**
+** Translate almanac packet to almanac structure
+**
+** @param [r] s [const UC *] almanac packet data
+** @param [w] alm [GPS_PAlmanac *] almanac entry pointer
+**
+** @return [void]
+************************************************************************/
+static void GPS_A500_Translate(UC *s, GPS_PAlmanac *alm)
+{
+ UC *p;
+
+ p=s;
+
+ (*alm)->wn = GPS_Util_Get_Short(p);
+ p+=sizeof(int16);
+
+ (*alm)->toa = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*alm)->af0 = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*alm)->af1 = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*alm)->e = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*alm)->sqrta = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*alm)->m0 = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*alm)->w = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*alm)->omg0 = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*alm)->odot = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*alm)->i = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ return;
+}
+
+
+/* @funcstatic GPS_D500_Send *******************************************
+**
+** Form almanac data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] alm [GPS_PAlmanac] almanac data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D500_Send(UC *data, GPS_PAlmanac alm)
+{
+ UC *p;
+
+ p = data;
+ GPS_A500_Encode(p,alm);
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D501_Send ********************************************
+**
+** Form almanac data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] alm [GPS_PAlmanac] almanac data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D501_Send(UC *data, GPS_PAlmanac alm)
+{
+ UC *p;
+
+ p=data;
+ p[42] = alm->hlth;
+ GPS_A500_Encode(p,alm);
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D550_Send ********************************************
+**
+** Form almanac data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] alm [GPS_PAlmanac] almanac data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D550_Send(UC *data, GPS_PAlmanac alm)
+{
+ UC *p;
+
+ p = data;
+ *p = alm->svid;
+ GPS_A500_Encode(p+1,alm);
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_D551_Send ********************************************
+**
+** Form almanac data string
+**
+** @param [w] data [UC *] string to write to
+** @param [r] alm [GPS_PAlmanac] almanac data
+**
+** @return [void]
+************************************************************************/
+static void GPS_D551_Send(UC *data, GPS_PAlmanac alm)
+{
+ UC *p;
+
+ p = data;
+ *p = alm->svid;
+ GPS_A500_Encode(p+1,alm);
+ p[43] = alm->hlth;
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_A500_Encode ***************************************
+**
+** Encode almanac structure to almanac packet
+**
+** @param [w] s [UC *] string to write to
+** @param [r] alm [GPS_PAlmanac] almanac entry
+**
+** @return [void]
+************************************************************************/
+static void GPS_A500_Encode(UC *s, GPS_PAlmanac alm)
+{
+ UC *p;
+
+ p=s;
+
+ GPS_Util_Put_Short(p,alm->wn);
+ p+=sizeof(int16);
+
+ GPS_Util_Put_Float(p,alm->toa);
+ p+=sizeof(float);
+
+ GPS_Util_Put_Float(p,alm->af0);
+ p+=sizeof(float);
+
+ GPS_Util_Put_Float(p,alm->af1);
+ p+=sizeof(float);
+
+ GPS_Util_Put_Float(p,alm->e);
+ p+=sizeof(float);
+
+ GPS_Util_Put_Float(p,alm->sqrta);
+ p+=sizeof(float);
+
+ GPS_Util_Put_Float(p,alm->m0);
+ p+=sizeof(float);
+
+ GPS_Util_Put_Float(p,alm->w);
+ p+=sizeof(float);
+
+ GPS_Util_Put_Float(p,alm->omg0);
+ p+=sizeof(float);
+
+ GPS_Util_Put_Float(p,alm->odot);
+ p+=sizeof(float);
+
+ GPS_Util_Put_Float(p,alm->i);
+
+ return;
+}
+
+
+/* @func GPS_A600_Get ******************************************************
+**
+** Get time from GPS
+**
+** @param [r] port [const char *] serial port
+**
+** @return [time_t] GPS time as unix system time, -ve if error
+************************************************************************/
+time_t GPS_A600_Get(const char *port)
+{
+ static UC data[2];
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ time_t ret;
+
+ if(!GPS_Serial_On(port, &fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+
+ GPS_Util_Put_Short(data,
+ COMMAND_ID[gps_device_command].Cmnd_Transfer_Time);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ switch(gps_date_time_type)
+ {
+ case pD600:
+ ret = GPS_D600_Get(rec);
+ break;
+ default:
+ GPS_Error("A600_Get: Unknown data/time protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return ret;
+}
+
+
+
+
+
+/* @func GPS_A600_Send **************************************************
+**
+** Send time to GPS
+**
+** @param [r] port [const char *] serial port
+** @param [r] Time [time_t] unix-style time
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_A600_Send(const char *port, time_t Time)
+{
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+ int32 posnsent=0;
+ int32 ret=0;
+
+ if(!GPS_Serial_On(port, &fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+
+ switch(gps_date_time_type)
+ {
+ case pD600:
+ GPS_D600_Send(&tra,Time);
+ break;
+ default:
+ GPS_Error("A600_Send: Unknown data/time protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_error;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ return gps_error;
+
+
+ /*
+ * Allow GPS a little while to decide whether it wants to ask for
+ * the position. Note that the posn sent is held in gps_save_lat
+ * and gps_save_lon globals!
+ */
+ if(GPS_Serial_Wait(fd))
+ {
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ if(rec->type == LINK_ID[gps_link_type].Pid_Command_Data &&
+ GPS_Util_Get_Short(rec->data) == COMMAND_ID[gps_device_command].
+ Cmnd_Transfer_Posn)
+ {
+ GPS_User("INFO: GPS position request. Sending....");
+ ret = GPS_Rqst_Send_Position(fd,gps_save_lat,gps_save_lon);
+ if(ret < 0) return ret;
+ posnsent=1;
+ }
+ }
+
+
+ if(!posnsent)
+ {
+ ret = GPS_Rqst_Send_Position(fd,gps_save_lat,gps_save_lon);
+ if(ret < 0) return ret;
+ }
+
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return 1;
+}
+
+
+
+
+
+/* @func GPS_D600_Get ******************************************************
+**
+** Convert date/time packet to ints
+**
+** @param [r] packet [GPS_PPacket] packet
+**
+** @return [time_t] gps time as unix system time
+************************************************************************/
+time_t GPS_D600_Get(GPS_PPacket packet)
+{
+ UC *p;
+ static struct tm ts;
+
+ p = packet->data;
+
+ ts.tm_mon = *p++ - 1;
+ ts.tm_mday = *p++;
+ ts.tm_year = (int32) GPS_Util_Get_Short(p) - 1900;
+ p+=2;
+ ts.tm_hour = (int32) GPS_Util_Get_Short(p);
+ p+=2;
+ ts.tm_min = *p++;
+ ts.tm_sec = *p++;
+
+ return mktime(&ts);
+}
+
+
+/* @func GPS_D600_Send ******************************************************
+**
+** make a time packet for sending to the GPS
+**
+** @param [w] packet [GPS_PPacket *] packet
+** @param [r] Time [time_t] unix-style time
+**
+** @return [void]
+************************************************************************/
+void GPS_D600_Send(GPS_PPacket *packet, time_t Time)
+{
+ UC data[10];
+ UC *p;
+ struct tm *ts;
+
+ p = data;
+
+ ts = localtime(&Time);
+ *p++ = ts->tm_mon+1;
+ *p++ = ts->tm_mday;
+
+ GPS_Util_Put_Short(p,ts->tm_year+1900);
+ p+=2;
+ GPS_Util_Put_Short(p,ts->tm_hour);
+ p+=2;
+
+ *p++ = ts->tm_min;
+ *p = ts->tm_sec;
+
+ GPS_Make_Packet(packet, LINK_ID[gps_link_type].Pid_Date_Time_Data,
+ data,8);
+
+ return;
+}
+
+
+
+
+/* @func GPS_A700_Get ******************************************************
+**
+** Get position from GPS
+**
+** @param [r] port [const char *] serial port
+** @param [w] lat [double *] latitude (deg)
+** @param [w] lon [double *] longitude (deg)
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_A700_Get(const char *port, double *lat, double *lon)
+{
+ static UC data[2];
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+
+ if(!GPS_Serial_On(port, &fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+
+ GPS_Util_Put_Short(data,
+ COMMAND_ID[gps_device_command].Cmnd_Transfer_Posn);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ if(!GPS_Packet_Read(fd, &rec))
+ return gps_errno;
+ if(!GPS_Send_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ switch(gps_position_type)
+ {
+ case pD700:
+ GPS_D700_Get(rec, lat, lon);
+ break;
+ default:
+ GPS_Error("A700_Get: Unknown position protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return 1;
+}
+
+
+
+/* @func GPS_A700_Send ******************************************************
+**
+** Send position to GPS
+**
+** @param [r] port [const char *] serial port
+** @param [r] lat [double] latitude (deg)
+** @param [r] lon [double] longitute (deg)
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_A700_Send(const char *port, double lat, double lon)
+{
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+
+ if(!GPS_Serial_On(port, &fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+
+ switch(gps_position_type)
+ {
+ case pD700:
+ GPS_D700_Send(&tra,lat,lon);
+ break;
+ default:
+ GPS_Error("A700_Send: Unknown position protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ if(!GPS_Write_Packet(fd,tra))
+ return 0;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ return 0;
+
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return 1;
+}
+
+
+
+/* @func GPS_D700_Get ******************************************************
+**
+** Convert position packet to lat/long in degrees
+**
+** @param [r] packet [GPS_PPacket] packet
+** @param [w] lat [double *] latitude (deg)
+** @param [w] lon [double *] longitude (deg)
+**
+** @return [void]
+************************************************************************/
+void GPS_D700_Get(GPS_PPacket packet, double *lat, double *lon)
+{
+ UC *p;
+ double t;
+
+ p = packet->data;
+
+ t = GPS_Util_Get_Double(p);
+ *lat = GPS_Math_Rad_To_Deg(t);
+
+ p += sizeof(double);
+
+ t = GPS_Util_Get_Double(p);
+ *lon = GPS_Math_Rad_To_Deg(t);
+
+
+ return;
+}
+
+
+/* @func GPS_D700_Send ******************************************************
+**
+** make a position packet for sending to the GPS
+**
+** @param [w] packet [GPS_PPacket *] packet
+** @param [r] lat [double] latitude (deg)
+** @param [r] lon [double] longitude (deg)
+**
+** @return [void]
+************************************************************************/
+void GPS_D700_Send(GPS_PPacket *packet, double lat, double lon)
+{
+ UC data[16];
+ UC *p;
+
+ lat = GPS_Math_Deg_To_Rad(lat);
+ lon = GPS_Math_Deg_To_Rad(lon);
+
+ p = data;
+
+ GPS_Util_Put_Double(p,lat);
+ p+=sizeof(double);
+ GPS_Util_Put_Double(p,lon);
+
+ GPS_Make_Packet(packet, LINK_ID[gps_link_type].Pid_Position_Data,
+ data,16);
+
+ return;
+}
+
+
+
+/* @func GPS_A800_On ******************************************************
+**
+** Turn on GPS PVT
+**
+** @param [r] port [const char *] serial port
+** @param [w] fd [int32 *] file descriptor
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_A800_On(const char *port, int32 *fd)
+{
+ static UC data[2];
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+
+ if(!GPS_Serial_On(port, fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+
+ GPS_Util_Put_Short(data,
+ COMMAND_ID[gps_device_command].Cmnd_Start_Pvt_Data);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
+ data,2);
+ if(!GPS_Write_Packet(*fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(*fd, &tra, &rec))
+ {
+ GPS_Error("A800_on: Pvt start data not acknowledged");
+ return FRAMING_ERROR;
+ }
+
+ GPS_Packet_Del(&rec);
+ GPS_Packet_Del(&tra);
+
+ return 1;
+}
+
+
+
+/* @func GPS_A800_Off ******************************************************
+**
+** Turn off GPS PVT
+**
+** @param [r] port [const char *] port
+** @param [w] fd [int32 *] file descriptor
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_A800_Off(const char *port, int32 *fd)
+{
+ static UC data[2];
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+
+ GPS_Util_Put_Short(data,
+ COMMAND_ID[gps_device_command].Cmnd_Stop_Pvt_Data);
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
+ data,2);
+ if(!GPS_Write_Packet(*fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(*fd, &tra, &rec))
+ {
+ GPS_Error("A800_Off: Not acknowledged");
+ return FRAMING_ERROR;
+ }
+
+
+ GPS_Packet_Del(&rec);
+ GPS_Packet_Del(&tra);
+
+ if(!GPS_Serial_Off(port, *fd))
+ return gps_errno;
+
+ return 1;
+}
+
+
+/* @func GPS_A800_Get **************************************************
+**
+** make a position packet for sending to the GPS
+**
+** @param [r] fd [int32 *] file descriptor
+** @param [w] packet [GPS_PPvt_Data *] packet
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_A800_Get(int32 *fd, GPS_PPvt_Data *packet)
+{
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+
+ if(!GPS_Packet_Read(*fd, &rec))
+ return gps_errno;
+
+ if(!GPS_Send_Ack(*fd, &tra, &rec))
+ return gps_errno;
+
+ switch(gps_pvt_type)
+ {
+ case pD800:
+ GPS_D800_Get(rec,packet);
+ break;
+ default:
+ GPS_Error("A800_GET: Unknown pvt protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ GPS_Packet_Del(&rec);
+ GPS_Packet_Del(&tra);
+
+ return 1;
+}
+
+
+
+/* @func GPS_D800_Get ******************************************************
+**
+** Convert packet to pvt structure
+**
+** @param [r] packet [GPS_PPacket] packet
+** @param [w] pvt [GPS_PPvt_Data *] pvt structure
+**
+** @return [void]
+************************************************************************/
+void GPS_D800_Get(GPS_PPacket packet, GPS_PPvt_Data *pvt)
+{
+ UC *p;
+
+ p = packet->data;
+
+ (*pvt)->alt = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*pvt)->epe = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*pvt)->eph = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*pvt)->epv = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*pvt)->fix = GPS_Util_Get_Short(p);
+ p+=sizeof(int16);
+
+ (*pvt)->tow = GPS_Util_Get_Double(p);
+ p+=sizeof(double);
+
+ (*pvt)->lat = GPS_Math_Rad_To_Deg(GPS_Util_Get_Double(p));
+ p+=sizeof(double);
+
+ (*pvt)->lon = GPS_Math_Rad_To_Deg(GPS_Util_Get_Double(p));
+ p+=sizeof(double);
+
+ (*pvt)->east = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*pvt)->north = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*pvt)->up = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*pvt)->msl_hght = GPS_Util_Get_Float(p);
+ p+=sizeof(float);
+
+ (*pvt)->leap_scnds = GPS_Util_Get_Short(p);
+ p+=sizeof(int16);
+
+ (*pvt)->wn_days = GPS_Util_Get_Int(p);
+
+ return;
+}
+
+
+
--- /dev/null
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef gpsapp_h
+#define gpsapp_h
+
+
+#include "gps.h"
+
+int32 GPS_Init(const char *port);
+
+int32 GPS_A100_Get(const char *port, GPS_PWay **way);
+int32 GPS_A100_Send(const char *port, GPS_PWay *way, int32 n);
+
+int32 GPS_A200_Get(const char *port, GPS_PWay **way);
+int32 GPS_A201_Get(const char *port, GPS_PWay **way);
+int32 GPS_A200_Send(const char *port, GPS_PWay *way, int32 n);
+int32 GPS_A201_Send(const char *port, GPS_PWay *way, int32 n);
+
+int32 GPS_A300_Get(const char *port, GPS_PTrack **trk);
+int32 GPS_A301_Get(const char *port, GPS_PTrack **trk);
+int32 GPS_A300_Send(const char *port, GPS_PTrack *trk, int32 n);
+int32 GPS_A301_Send(const char *port, GPS_PTrack *trk, int32 n);
+
+int32 GPS_D300_Get(GPS_PTrack *trk, int32 entries, int32 fd);
+void GPS_D300b_Get(GPS_PTrack *trk, UC *data);
+void GPS_D301b_Get(GPS_PTrack *trk, UC *data);
+void GPS_D310_Get(GPS_PTrack *trk, UC *s);
+void GPS_D300_Send(UC *data, GPS_PTrack trk);
+void GPS_D301_Send(UC *data, GPS_PTrack trk);
+void GPS_D310_Send(UC *data, GPS_PTrack trk, int32 *len);
+
+int32 GPS_A400_Get(const char *port, GPS_PWay **way);
+int32 GPS_A400_Send(const char *port, GPS_PWay *way, int32 n);
+
+int32 GPS_A500_Get(const char *port, GPS_PAlmanac **alm);
+int32 GPS_A500_Send(const char *port, GPS_PAlmanac *alm, int32 n);
+
+time_t GPS_A600_Get(const char *port);
+time_t GPS_D600_Get(GPS_PPacket packet);
+int32 GPS_A600_Send(const char *port, time_t Time);
+void GPS_D600_Send(GPS_PPacket *packet, time_t Time);
+
+int32 GPS_A700_Get(const char *port, double *lat, double *lon);
+int32 GPS_A700_Send(const char *port, double lat, double lon);
+void GPS_D700_Get(GPS_PPacket packet, double *lat, double *lon);
+void GPS_D700_Send(GPS_PPacket *packet, double lat, double lon);
+
+int32 GPS_A800_On(const char *port, int32 *fd);
+int32 GPS_A800_Off(const char *port, int32 *fd);
+int32 GPS_A800_Get(int32 *fd, GPS_PPvt_Data *packet);
+void GPS_D800_Get(GPS_PPacket packet, GPS_PPvt_Data *pvt);
+
+
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null
+/********************************************************************
+** @source JEEPS command functions
+**
+** @author Copyright (C) 1999 Alan Bleasby
+** @version 1.0
+** @modified Dec 28 1999 Alan Bleasby. First version
+** @@
+**
+** This library is free software; you can redistribute it and/or
+** modify it under the terms of the GNU Library General Public
+** License as published by the Free Software Foundation; either
+** version 2 of the License, or (at your option) any later version.
+**
+** This library is distributed in the hope that it will be useful,
+** but WITHOUT ANY WARRANTY; without even the implied warranty of
+** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+** Library General Public License for more details.
+**
+** You should have received a copy of the GNU Library General Public
+** License along with this library; if not, write to the
+** Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+** Boston, MA 02111-1307, USA.
+********************************************************************/
+#include "gps.h"
+#include <stdio.h>
+
+
+/* @func GPS_Command_Off ***********************************************
+**
+** Turn off power on GPS
+**
+** @return [int32] success
+************************************************************************/
+
+int32 GPS_Command_Off(const char *port)
+{
+ static UC data[2];
+ int32 fd;
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+
+ if(!GPS_Serial_On(port, &fd))
+ return gps_errno;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+ GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Turn_Off_Pwr);
+
+ GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
+ data,2);
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+
+ if(!GPS_Serial_Chars_Ready(fd))
+ {
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ return gps_errno;
+ GPS_User("Power off command acknowledged");
+ }
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ if(!GPS_Serial_Off(port, fd))
+ return gps_errno;
+
+ return 1;
+}
+
+
+/* @func GPS_Command_Get_Waypoint ***************************************
+**
+** Get waypoint from GPS
+**
+** @param [r] port [const char *] serial port
+** @param [w] way [GPS_PWay **] pointer to waypoint array
+**
+** @return [int32] number of waypoint entries
+************************************************************************/
+
+int32 GPS_Command_Get_Waypoint(const char *port, GPS_PWay **way)
+{
+ int32 ret=0;
+
+ switch(gps_waypt_transfer)
+ {
+ case pA100:
+ ret = GPS_A100_Get(port,way);
+ break;
+ default:
+ GPS_Error("Get_Waypoint: Unknown waypoint protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ return ret;
+}
+
+
+
+/* @func GPS_Command_Send_Waypoint ******************************************
+**
+** Send waypoints to GPS
+**
+** @param [r] port [const char *] serial port
+** @param [r] way [GPS_PWay *] waypoint array
+** @param [r] n [int32] number of waypoint entries
+**
+** @return [int32] success
+************************************************************************/
+
+int32 GPS_Command_Send_Waypoint(const char *port, GPS_PWay *way, int32 n)
+{
+ int32 ret=0;
+
+ switch(gps_waypt_transfer)
+ {
+ case pA100:
+ ret = GPS_A100_Send(port, way, n);
+ break;
+ default:
+ GPS_Error("Send_Waypoint: Unknown waypoint protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ return ret;
+}
+
+
+/* @func GPS_Command_Get_Route **************************************
+**
+** Get Route(s) from GPS
+**
+** @param [r] port [const char *] serial port
+** @param [w] way [GPS_PWay **] pointer to waypoint array
+**
+** @return [int32] number of waypoint entries
+************************************************************************/
+
+int32 GPS_Command_Get_Route(const char *port, GPS_PWay **way)
+{
+ int32 ret=0;
+
+ switch(gps_route_transfer)
+ {
+ case pA200:
+ ret = GPS_A200_Get(port,way);
+ break;
+ case pA201:
+ ret = GPS_A201_Get(port,way);
+ break;
+ default:
+ GPS_Error("Get_Route: Unknown route protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ return ret;
+}
+
+
+
+/* @func GPS_Command_Send_Route ****************************************
+**
+** Send route(s) to GPS
+**
+** @param [r] port [const char *] serial port
+** @param [r] way [GPS_PWay *] waypoint array
+** @param [r] n [int32] number of waypoint entries
+**
+** @return [int32] success
+************************************************************************/
+
+int32 GPS_Command_Send_Route(const char *port, GPS_PWay *way, int32 n)
+{
+ int32 ret=0;
+
+
+ switch(gps_route_transfer)
+ {
+ case pA200:
+ ret = GPS_A200_Send(port, way, n);
+ break;
+ case pA201:
+ ret = GPS_A201_Send(port, way, n);
+ break;
+ default:
+ GPS_Error("Send_Route: Unknown route protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ return ret;
+}
+
+
+/* @func GPS_Command_Get_Track ***************************************
+**
+** Get track log from GPS
+**
+** @param [r] port [const char *] serial port
+** @param [w] trk [GPS_PTrack **] pointer to track array
+**
+** @return [int32] number of track entries
+************************************************************************/
+
+int32 GPS_Command_Get_Track(const char *port, GPS_PTrack **trk)
+{
+ int32 ret=0;
+
+ if(gps_trk_transfer == -1)
+ return GPS_UNSUPPORTED;
+
+ switch(gps_trk_transfer)
+ {
+ case pA300:
+ ret = GPS_A300_Get(port,trk);
+ break;
+ case pA301:
+ ret = GPS_A301_Get(port,trk);
+ break;
+ default:
+ GPS_Error("Get_Track: Unknown track protocol\n");
+ return PROTOCOL_ERROR;
+ }
+
+ return ret;
+}
+
+
+
+/* @func GPS_Command_Send_Track ******************************************
+**
+** Send track log to GPS
+**
+** @param [r] port [const char *] serial port
+** @param [r] trk [GPS_PTrack *] track array
+** @param [r] n [int32] number of track entries
+**
+** @return [int32] success
+************************************************************************/
+
+int32 GPS_Command_Send_Track(const char *port, GPS_PTrack *trk, int32 n)
+{
+ int32 ret=0;
+
+ if(gps_trk_transfer == -1)
+ return GPS_UNSUPPORTED;
+
+ switch(gps_trk_transfer)
+ {
+ case pA300:
+ ret = GPS_A300_Send(port, trk, n);
+ break;
+ case pA301:
+ ret = GPS_A301_Send(port, trk, n);
+ break;
+ default:
+ GPS_Error("Send_Track: Unknown track protocol");
+ break;
+ }
+
+ return ret;
+}
+
+
+/* @func GPS_Command_Get_Proximity **************************************
+**
+** Get proximitywaypoint from GPS
+**
+** @param [r] port [const char *] serial port
+** @param [w] way [GPS_PWay **] pointer to waypoint array
+**
+** @return [int32] number of waypoint entries
+************************************************************************/
+
+int32 GPS_Command_Get_Proximity(const char *port, GPS_PWay **way)
+{
+ int32 ret=0;
+
+ if(gps_prx_waypt_transfer == -1)
+ return GPS_UNSUPPORTED;
+
+ switch(gps_prx_waypt_transfer)
+ {
+ case pA400:
+ ret = GPS_A400_Get(port,way);
+ break;
+ default:
+ GPS_Error("Get_Proximity: Unknown proximity protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ return ret;
+}
+
+
+
+/* @func GPS_Command_Send_Proximity ******************************************
+**
+** Send proximity waypoints to GPS
+**
+** @param [r] port [const char *] serial port
+** @param [r] way [GPS_PWay *] waypoint array
+** @param [r] n [int32] number of waypoint entries
+**
+** @return [int32] success
+************************************************************************/
+
+int32 GPS_Command_Send_Proximity(const char *port, GPS_PWay *way, int32 n)
+{
+ int32 ret=0;
+
+
+ if(gps_prx_waypt_transfer == -1)
+ return GPS_UNSUPPORTED;
+
+
+ switch(gps_prx_waypt_transfer)
+ {
+ case pA400:
+ ret = GPS_A400_Send(port, way, n);
+ break;
+ default:
+ GPS_Error("Send_Proximity: Unknown proximity protocol");
+ break;
+ }
+
+ return ret;
+}
+
+
+
+/* @func GPS_Command_Get_Almanac ***************************************
+**
+** Get almanac from GPS
+**
+** @param [r] port [const char *] serial port
+** @param [w] alm [GPS_PAlmanac **] pointer to almanac array
+**
+** @return [int32] number of almanac entries
+************************************************************************/
+
+int32 GPS_Command_Get_Almanac(const char *port, GPS_PAlmanac **alm)
+{
+ int32 ret=0;
+
+ switch(gps_almanac_transfer)
+ {
+ case pA500:
+ ret = GPS_A500_Get(port,alm);
+ break;
+ default:
+ GPS_Error("Get_Almanac: Unknown almanac protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ return ret;
+}
+
+
+
+/* @func GPS_Command_Send_Almanac ******************************************
+**
+** Send almanac to GPS
+**
+** @param [r] port [const char *] serial port
+** @param [r] alm [GPS_PAlmanac *] almanac array
+** @param [r] n [int32] number of almanac entries
+**
+** @return [int32] success
+************************************************************************/
+
+int32 GPS_Command_Send_Almanac(const char *port, GPS_PAlmanac *alm, int32 n)
+{
+ int32 ret=0;
+
+ switch(gps_almanac_transfer)
+ {
+ case pA500:
+ ret = GPS_A500_Send(port, alm, n);
+ break;
+ default:
+ GPS_Error("Send_Almanac: Unknown almanac protocol");
+ break;
+ }
+
+ return ret;
+}
+
+
+
+/* @func GPS_Command_Get_Time ******************************************
+**
+** Get time from GPS
+**
+** @param [r] port [const char *] serial port
+**
+** @return [time_t] unix-style time
+************************************************************************/
+
+time_t GPS_Command_Get_Time(const char *port)
+{
+ time_t ret=0;
+
+ switch(gps_date_time_transfer)
+ {
+ case pA600:
+ ret = GPS_A600_Get(port);
+ break;
+ default:
+ GPS_Error("Get_Time: Unknown date/time protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ return ret;
+}
+
+
+
+/* @func GPS_Command_Send_Time ******************************************
+**
+** Set GPS time
+**
+** @param [r] port [const char *] serial port
+** @param [r] Time [time_t] unix-style time
+**
+** @return [int32] true if OK
+************************************************************************/
+
+int32 GPS_Command_Send_Time(const char *port, time_t Time)
+{
+ time_t ret=0;
+
+ switch(gps_date_time_transfer)
+ {
+ case pA600:
+ ret = GPS_A600_Send(port, Time);
+ break;
+ default:
+ GPS_Error("Send_Time: Unknown date/time protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ return ret;
+}
+
+
+
+
+/* @func GPS_Command_Get_Position ***************************************
+**
+** Get position from GPS
+**
+** @param [r] port [const char *] serial port
+** @param [w] lat [double *] latitude (deg)
+** @param [w] lon [double *] longitude (deg)
+**
+** @return [int32] success
+************************************************************************/
+
+int32 GPS_Command_Get_Position(const char *port, double *lat, double *lon)
+{
+ int32 ret=0;
+
+ switch(gps_position_transfer)
+ {
+ case pA700:
+ ret = GPS_A700_Get(port,lat,lon);
+ break;
+ default:
+ GPS_Error("Get_Position: Unknown position protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ return ret;
+}
+
+
+
+/* @func GPS_Command_Send_Position ******************************************
+**
+** Set GPS position
+**
+** @param [r] port [const char *] serial port
+** @param [r] lat [double] latitude (deg)
+** @param [r] lon [double] longitude (deg)
+**
+** @return [int32] success
+************************************************************************/
+
+int32 GPS_Command_Send_Position(const char *port, double lat, double lon)
+{
+ int32 ret=0;
+
+ switch(gps_position_transfer)
+ {
+ case pA700:
+ ret = GPS_A700_Send(port, lat, lon);
+ break;
+ default:
+ GPS_Error("Send_Position: Unknown position protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ return ret;
+}
+
+
+/* @func GPS_Command_Pvt_On ********************************************
+**
+** Instruct GPS to start sending Pvt data every second
+**
+** @param [r] port [const char *] serial port
+** @param [w] fd [int32 *] file descriptor
+**
+** @return [int32] success if supported and GPS starts sending
+************************************************************************/
+
+int32 GPS_Command_Pvt_On(const char *port, int32 *fd)
+{
+ int32 ret=0;
+
+
+ if(gps_pvt_transfer == -1)
+ return GPS_UNSUPPORTED;
+
+
+ switch(gps_pvt_transfer)
+ {
+ case pA800:
+ ret = GPS_A800_On(port,fd);
+ break;
+ default:
+ GPS_Error("Pvt_On: Unknown position protocol");
+ return PROTOCOL_ERROR;
+ }
+
+
+ return ret;
+}
+
+
+
+/* @func GPS_Command_Pvt_Off ********************************************
+**
+** Instruct GPS to stop sending Pvt data every second
+**
+** @param [r] port [const char *] serial port
+** @param [w] fd [int32 *] file descriptor
+**
+** @return [int32] success
+************************************************************************/
+
+int32 GPS_Command_Pvt_Off(const char *port, int32 *fd)
+{
+ int32 ret=0;
+
+
+ if(gps_pvt_transfer == -1)
+ return GPS_UNSUPPORTED;
+
+ switch(gps_pvt_transfer)
+ {
+ case pA800:
+ ret = GPS_A800_Off(port,fd);
+ break;
+ default:
+ GPS_Error("Pvt_Off: Unknown position protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ return ret;
+}
+
+
+
+/* @func GPS_Command_Pvt_Get ********************************************
+**
+** Get a single PVT info entry
+**
+** @param [w] fd [int32 *] file descriptor
+** @param [w] pvt [GPS_PPvt_Data *] pvt data structure to fill
+**
+** @return [int32] success
+************************************************************************/
+
+int32 GPS_Command_Pvt_Get(int32 *fd, GPS_PPvt_Data *pvt)
+{
+ int32 ret=0;
+
+ if(gps_pvt_transfer == -1)
+ return GPS_UNSUPPORTED;
+
+ (*pvt)->fix = 0;
+
+ switch(gps_pvt_transfer)
+ {
+ case pA800:
+ ret = GPS_A800_Get(fd,pvt);
+ break;
+ default:
+ GPS_Error("Pvt_Get: Unknown position protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ return ret;
+}
--- /dev/null
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef gpscom_h
+#define gpscom_h
+
+
+#include "gps.h"
+#include <time.h>
+
+int32 GPS_Command_Off(const char *port);
+
+time_t GPS_Command_Get_Time(const char *port);
+int32 GPS_Command_Send_Time(const char *port, time_t Time);
+
+int32 GPS_Command_Get_Position(const char *port, double *lat, double *lon);
+int32 GPS_Command_Send_Position(const char *port, double lat, double lon);
+
+int32 GPS_Command_Pvt_On(const char *port, int32 *fd);
+int32 GPS_Command_Pvt_Off(const char *port, int32 *fd);
+int32 GPS_Command_Pvt_Get(int32 *fd, GPS_PPvt_Data *pvt);
+
+int32 GPS_Command_Get_Almanac(const char *port, GPS_PAlmanac **alm);
+int32 GPS_Command_Send_Almanac(const char *port, GPS_PAlmanac *alm, int32 n);
+
+int32 GPS_Command_Get_Track(const char *port, GPS_PTrack **trk);
+int32 GPS_Command_Send_Track(const char *port, GPS_PTrack *trk, int32 n);
+
+int32 GPS_Command_Get_Waypoint(const char *port, GPS_PWay **way);
+int32 GPS_Command_Send_Waypoint(const char *port, GPS_PWay *way, int32 n);
+
+int32 GPS_Command_Get_Proximity(const char *port, GPS_PWay **way);
+int32 GPS_Command_Send_Proximity(const char *port, GPS_PWay *way, int32 n);
+
+int32 GPS_Command_Get_Route(const char *port, GPS_PWay **way);
+int32 GPS_Command_Send_Route(const char *port, GPS_PWay *way, int32 n);
+
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef gpsdatum_h
+#define gpsdatum_h
+
+
+
+typedef struct GPS_SEllipse
+{
+ char *name;
+ double a;
+ double invf;
+} GPS_OEllipse, *GPS_PEllipse;
+
+GPS_OEllipse GPS_Ellipse[]=
+{
+ { "Airy 1830", 6377563.396, 299.3249646 },
+ { "Airy 1830 Modified", 6377340.189, 299.3249646 },
+ { "Australian National", 6378160.000, 298.25 },
+ { "Bessel 1841 (Namibia)", 6377483.865, 299.1528128 },
+ { "Bessel 1841", 6377397.155, 299.1528128 },
+ { "Clarke 1866", 6378206.400, 294.9786982 },
+ { "Clarke 1880", 6378249.145, 293.465 },
+ { "Everest (India 1830)", 6377276.345, 300.8017 },
+ { "Everest (Sabah Sarawak)", 6377298.556, 300.8017 },
+ { "Everest (India 1956)", 6377301.243, 300.8017 },
+ { "Everest (Malaysia 1969)", 6377295.664, 300.8017 },
+ { "Everest (Malay & Sing)", 6377304.063, 300.8017 },
+ { "Everest (Pakistan)", 6377309.613, 300.8017 },
+ { "Modified Fischer 1960", 6378155.000, 298.3 },
+ { "Helmert 1906", 6378200.000, 298.3 },
+ { "Hough 1960", 6378270.000, 297.0 },
+ { "Indonesian 1974", 6378160.000, 298.247 },
+ { "International 1924", 6378388.000, 297.0 },
+ { "Krassovsky 1940", 6378245.000, 298.3 },
+ { "GRS67", 6378160.000, 6356774.516 },
+ { "GRS75", 6378140.000, 6356755.288 },
+ { "GRS80", 6378137.000, 298.257222101 },
+ { "S. American 1969", 6378160.000, 298.25 },
+ { "WGS60", 6378165.000, 298.3 },
+ { "WGS66", 6378145.000, 298.25 },
+ { "WGS72", 6378135.000, 298.26 },
+ { "WGS84", 6378137.000, 298.257223563 }
+};
+
+
+
+typedef struct GPS_SDatum
+{
+ char *name;
+ int ellipse;
+ double dx;
+ double dy;
+ double dz;
+} GPS_ODatum, *GPS_PDatum;
+
+GPS_ODatum GPS_Datum[]=
+{
+/* 000 */ { "Adindan", 6, -166, -15, 204 },
+/* 001 */ { "AFG", 18, -43, -163, 45 },
+/* 002 */ { "Ain-El-Abd", 17, -150, -251, -2 },
+/* 003 */ { "Alaska-NAD27", 5, -5, 135, 172 },
+/* 004 */ { "Alaska-Canada", 6, -9, 151, 185 },
+/* 005 */ { "Anna-1-Astro", 2, -491, -22, 435 },
+/* 006 */ { "ARC 1950 Mean", 6, -143, -90, -294 },
+/* 007 */ { "ARC 1960 Mean", 6, -160, -8, -300 },
+/* 008 */ { "Asc Island 58", 17, -207, 107, 52 },
+/* 009 */ { "Astro B4", 17, 114, -116, -333 },
+/* 010 */ { "Astro Beacon E", 17, 145, 75, -272 },
+/* 011 */ { "Astro pos 71/4", 17, -320, 550, -494 },
+/* 012 */ { "Astro stn 52", 17, 124, -234, -25 },
+/* 013 */ { "Australia Geo 1984", 2, -134, -48, 149 },
+/* 014 */ { "Bahamas NAD27", 6, -4, 154, 178 },
+/* 015 */ { "Bellevue IGN", 17, -127, -769, 472 },
+/* 016 */ { "Bermuda 1957", 6, -73, 213, 296 },
+/* 017 */ { "Bukit Rimpah", 4, -384, 664, -48 },
+/* 018 */ { "Camp_Area_Astro", 17, -104, -129, 239 },
+/* 019 */ { "Campo_Inchauspe", 17, -148, 136, 90 },
+/* 020 */ { "Canada_Mean(NAD27)", 5, -10, 158, 187 },
+/* 021 */ { "Canal_Zone_(NAD27)", 5, 0, 125, 201 },
+/* 022 */ { "Canton_Island_1966", 17, 298, -304, -375 },
+/* 023 */ { "Cape", 6, -136, -108, -292 },
+/* 024 */ { "Cape_Canaveral_mean", 5, -2, 150, 181 },
+/* 025 */ { "Carribean NAD27", 5, -7, 152, 178 },
+/* 026 */ { "Carthage", 6, -263, 6, 431 },
+/* 027 */ { "Cent America NAD27", 5 , 0, 125, 194 },
+/* 028 */ { "Chatham 1971", 17, 175, -38, 113 },
+/* 029 */ { "Chua Astro", 17, -134, 229, -29 },
+/* 030 */ { "Corrego Alegre", 17, -206, 172, -6 },
+/* 031 */ { "Cuba NAD27", 5, -9, 152, 178 },
+/* 032 */ { "Cyprus", 17, -104, -101, -140 },
+/* 033 */ { "Djakarta(Batavia)", 4, -377, 681, -50 },
+/* 034 */ { "DOS 1968", 17, 230, -199, -752 },
+/* 035 */ { "Easter lsland 1967", 17, 211, 147, 111 },
+/* 036 */ { "Egypt", 17, -130, -117, -151 },
+/* 037 */ { "European 1950", 17, -87, -96, -120 },
+/* 038 */ { "European 1950 mean", 17, -87, -98, -121 },
+/* 039 */ { "European 1979 mean", 17, -86, -98, -119 },
+/* 040 */ { "Finnish Nautical", 17, -78, -231, -97 },
+/* 041 */ { "Gandajika Base", 17, -133, -321, 50 },
+/* 042 */ { "Geodetic Datum 49", 17, 84, -22, 209 },
+/* 043 */ { "Ghana", 26, 0, 0, 0 },
+/* 044 */ { "Greenland NAD27", 5, 11, 114, 195 },
+/* 045 */ { "Guam 1963", 5, -100, -248, 259 },
+/* 046 */ { "Gunung Segara", 4, -403, 684, 41 },
+/* 047 */ { "Gunung Serindung 1962", 26, 0, 0, 0 },
+/* 048 */ { "GUX1 Astro", 17, 252, -209, -751 },
+/* 049 */ { "Herat North", 17, -333, -222, 114 },
+/* 050 */ { "Hjorsey 1955", 17, -73, 46, 86 },
+/* 051 */ { "Hong Kong 1963", 17, -156, -271, -189 },
+/* 052 */ { "Hu-Tzu-Shan", 17, -634, -549, -201 },
+/* 053 */ { "Indian", 9, 289, 734, 257 },
+/* 054 */ { "Iran", 17, -117, -132, -164 },
+/* 055 */ { "Ireland 1965", 1, 506, -122, 611 },
+/* 056 */ { "ISTS 073 Astro 69", 17, 208, -435, -229 },
+/* 057 */ { "Johnston Island 61", 17, 191, -77, -204 },
+/* 058 */ { "Kandawala", 7, -97, 787, 86 },
+/* 059 */ { "Kerguelen Island", 17, 145, -187, 103 },
+/* 060 */ { "Kertau 48", 11, -11, 851, 5 },
+/* 061 */ { "L.C. 5 Astro", 5, 42, 124, 147 },
+/* 062 */ { "La Reunion", 17, 94, -948, -1262 },
+/* 063 */ { "Liberia 1964", 6, -90, 40, 88 },
+/* 064 */ { "Luzon", 5, -133, -77, -51 },
+/* 065 */ { "Mahe 1971", 6, 41, -220, -134 },
+/* 066 */ { "Marco Astro", 17, -289, -124, 60 },
+/* 067 */ { "Masirah Is. Nahrwan", 6, -247, -148, 369 },
+/* 068 */ { "Massawa", 4, 639, 405, 60 },
+/* 069 */ { "Merchich", 6, 31, 146, 47 },
+/* 070 */ { "Mexico NAD27", 5, -12, 130, 190 },
+/* 071 */ { "Midway Astro 61", 17, 912, -58, 1227 },
+/* 072 */ { "Mindanao", 5, -133, -79, -72 },
+/* 073 */ { "Minna", 6, -92, -93, 122 },
+/* 074 */ { "Montjong Lowe", 26, 0, 0, 0 },
+/* 075 */ { "Nahrwan", 6, -231, -196, 482 },
+/* 076 */ { "Naparima BWI", 17, -2, 374, 172 },
+/* 077 */ { "North America 83", 21, 0, 0, 0 },
+/* 078 */ { "N. America 1927 mean", 5, -8, 160, 176 },
+/* 079 */ { "Observatorio 1966", 17, -425, -169, 81 },
+/* 080 */ { "Old Egyptian", 14, -130, 110, -13 },
+/* 081 */ { "Old Hawaiian_mean", 5, 89, -279, -183 },
+/* 082 */ { "Old Hawaiian Kauai", 5, 45, -290, -172 },
+/* 083 */ { "Old Hawaiian Maui", 5, 65, -290, -190 },
+/* 084 */ { "Old Hawaiian Oahu", 5, 56, -284, -181 },
+/* 085 */ { "Oman", 6, -346, -1, 224 },
+/* 086 */ { "OSGB36", 0, 375, -111, 431 },
+/* 087 */ { "Pico De Las Nieves", 17, -307, -92, 127 },
+/* 088 */ { "Pitcairn Astro 67", 17, 185, 165, 42 },
+/* 089 */ { "S. Am. 1956 mean(P)", 17, -288, 175, -376 },
+/* 090 */ { "S. Chilean 1963 (P)", 17, 16, 196, 93 },
+/* 091 */ { "Puerto Rico", 5, 11, 72, -101 },
+/* 092 */ { "Pulkovo 1942", 18, 28, -130, -95 },
+/* 093 */ { "Qornoq", 17, 164, 138, -189 },
+/* 094 */ { "Quatar National", 17, -128, -283, 22 },
+/* 095 */ { "Rome 1940", 17, -225, -65, 9 },
+/* 096 */ { "S-42(Pulkovo1942)", 18, 28, -121, -77 },
+/* 097 */ { "S.E.Asia_(Indian)", 7, 173, 750, 264 },
+/* 098 */ { "SAD-69/Brazil", 22, -60, -2, -41 },
+/* 099 */ { "Santa Braz", 17, -203, 141, 53 },
+/* 100 */ { "Santo (DOS)", 17, 170, 42, 84 },
+/* 101 */ { "Sapper Hill 43", 17, -355, 16, 74 },
+/* 102 */ { "Schwarzeck", 3, 616, 97, -251 },
+/* 103 */ { "Sicily", 17, -97, -88, -135 },
+/* 104 */ { "Sierra Leone 1960", 26, 0, 0, 0 },
+/* 105 */ { "S. Am. 1969 mean", 22, -57, 1, -41 },
+/* 106 */ { "South Asia", 13, 7, -10, -26 },
+/* 107 */ { "Southeast Base", 17, -499, -249, 314 },
+/* 108 */ { "Southwest Base", 17, -104, 167, -38 },
+/* 109 */ { "Tananarive Obs 25", 17, -189, -242, -91 },
+/* 110 */ { "Thai/Viet (Indian)", 7, 214, 836, 303 },
+/* 111 */ { "Timbalai 1948", 7, -689, 691, -45 },
+/* 112 */ { "Tokyo mean", 4, -128, 481, 664 },
+/* 113 */ { "Tristan Astro 1968", 17, -632, 438, -609 },
+/* 114 */ { "Unites Arab Emirates", 6, -249, -156, 381 },
+/* 115 */ { "Viti Levu 1916", 6, 51, 391, -36 },
+/* 116 */ { "Wake Eniwetok 60", 15, 101, 52, -39 },
+/* 117 */ { "WGS 72", 25, 0, 0, 5 },
+/* 118 */ { "WGS 84", 26, 0, 0, 0 },
+/* 119 */ { "Yacare", 17, -155, 171, 37 },
+/* 120 */ { "Zanderij", 17, -265, 120, -358 },
+/* 121 */ { "Sweden", 4, 424.3, -80.5, 613.1 },
+ { NULL, 0, 0, 0, 0 }
+};
+
+
+/* UK Ordnance Survey Nation Grid Map Codes */
+static char *UKNG[]=
+{
+ "SV","SW","SX","SY","SZ","TV","TW","SQ","SR","SS","ST","SU","TQ","TR",
+ "SL","SM","SN","SO","SP","TL","TM","SF","SG","SH","SJ","SK","TF","TG",
+ "SA","SB","SC","SD","SE","TA","TB","NV","NW","NX","NY","NZ","OV","OW",
+ "NQ","NR","NS","NT","NU","OQ","OR","NL","NM","NN","NO","NP","OL","OM",
+ "NF","NG","NH","NJ","NK","OF","OG","NA","NB","NC","ND","NE","OA","OB",
+ "HV","HW","HX","HY","HZ","JV","JW","HQ","HR","HS","HT","HU","JQ","JR",
+ "HL","HM","HN","HO","HP","JL","JM",""
+};
+
+
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null
+/********************************************************************
+** @source JEEPS output functions
+**
+** @author Copyright (C) 1999 Alan Bleasby
+** @version 1.0
+** @modified Dec 28 1999 Alan Bleasby. First version
+** @@
+**
+** This library is free software; you can redistribute it and/or
+** modify it under the terms of the GNU Library General Public
+** License as published by the Free Software Foundation; either
+** version 2 of the License, or (at your option) any later version.
+**
+** This library is distributed in the hope that it will be useful,
+** but WITHOUT ANY WARRANTY; without even the implied warranty of
+** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+** Library General Public License for more details.
+**
+** You should have received a copy of the GNU Library General Public
+** License along with this library; if not, write to the
+** Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+** Boston, MA 02111-1307, USA.
+********************************************************************/
+#include "gps.h"
+#include <stdio.h>
+#include <time.h>
+
+
+static void GPS_Fmt_Print_Way100(GPS_PWay way, FILE *outf);
+static void GPS_Fmt_Print_Way101(GPS_PWay way, FILE *outf);
+static void GPS_Fmt_Print_Way102(GPS_PWay way, FILE *outf);
+static void GPS_Fmt_Print_Way103(GPS_PWay way, FILE *outf);
+static void GPS_Fmt_Print_Way104(GPS_PWay way, FILE *outf);
+static void GPS_Fmt_Print_Way105(GPS_PWay way, FILE *outf);
+static void GPS_Fmt_Print_Way106(GPS_PWay way, FILE *outf);
+static void GPS_Fmt_Print_Way107(GPS_PWay way, FILE *outf);
+static void GPS_Fmt_Print_Way108(GPS_PWay way, FILE *outf);
+static void GPS_Fmt_Print_Way150(GPS_PWay way, FILE *outf);
+static void GPS_Fmt_Print_Way151(GPS_PWay way, FILE *outf);
+static void GPS_Fmt_Print_Way152(GPS_PWay way, FILE *outf);
+static void GPS_Fmt_Print_Way154(GPS_PWay way, FILE *outf);
+static void GPS_Fmt_Print_Way155(GPS_PWay way, FILE *outf);
+
+static void GPS_Fmt_Print_Track301(GPS_PTrack *trk, int32 n, FILE *outf);
+static void GPS_Fmt_Print_D300(GPS_PTrack trk, FILE *outf);
+static void GPS_Fmt_Print_D301(GPS_PTrack trk, FILE *outf);
+
+static int32 GPS_Fmt_Print_Route201(GPS_PWay *way, int32 n, FILE *outf);
+
+
+char *gps_marine_sym[]=
+{
+ "Anchor","Bell","Diamond-grn","Diamond_red","Dive1","Dive2","Dollar",
+ "Fish","Fuel","Horn","House","Knife","Light","Mug","Skull",
+ "Square_grn","Square_red","Wbuoy","Wpt_dot","Wreck","Null","Mob",
+
+ "Buoy_amber","Buoy_blck","Buoy_blue","Buoy_grn","Buoy_grn_red",
+ "Buoy_grn_wht","Buoy_orng","Buoy_red","Buoy_red_grn","Buoy_red_wht",
+ "Buoy_violet","Buoy_wht","Buoy_wht_grn","Buoy_wht_red","Dot","Rbcn",
+
+ "","","","","","","","","","",
+ "","","","","","","","","","",
+ "","","","","","","","","","",
+ "","","","","","","","","","",
+ "","","","","","","","","","",
+ "","","","","","","","","","",
+ "","","","","","","","","","",
+ "","","","","","","","","","",
+ "","","","","","","","","","",
+ "","","","","","","","","","",
+ "","","","","","","","","","",
+ "","",
+
+ "Boat_ramp","Camp","Toilets","Showers","Drinking_wtr","Phone",
+ "1st_aid","Info","Parking","Park","Picnic","Scenic","Skiing",
+ "Swimming","Dam","Controlled","Danger","Restricted","Null_2","Ball",
+ "Car","Deer","Shpng_trolley","Lodging","Mine","Trail_head",
+ "Lorry_stop","User_exit","Flag","Circle-x"
+};
+
+
+
+char *gps_land_sym[]=
+{
+ "Is_hwy","Us_hwy","St_hwy","Mi_mrkr","Trcbck","Golf","Sml_cty",
+ "Med_cty","Lrg_cty","Freeway","Ntl_hwy","Cap_cty","Amuse_pk",
+ "Bowling","Car_rental","Car_repair","Fastfood","Fitness","Film",
+ "Museum","Chemist","Pizza","Post_ofc","Rv_park","School",
+ "Stadium","Shop","Zoo","Petrol_plus","Theatre","Ramp_int",
+ "St_int","","","Weigh_stn","Toll_booth","Elev_pt","Ex_no_srvc",
+ "Geo_place_mm","Geo_place_wtr","Geo_place_lnd","Bridge","Building",
+ "Cemetery","Church","Civil_loc","Crossing","Hist_town","River_Embankment",
+ "Military_loc","Oil_field","Tunnel","Beach","Forest","Summit",
+ "Lrg_ramp_int","Lrg_exit_no_srvc","Official_badge","Gambling",
+ "Snow_ski","Ice_ski","Tow_truck","Border"
+};
+
+
+char *gps_aviation_sym[]=
+{
+ "Airport","Int","Ndb","Vor","Heliport","Private","Soft_fld",
+ "Tall_tower","Short_tower","Glider","Ultralight","Parachute",
+ "Vortac","Vordme","Faf","Lom","Map","Tacan","Seaplane"
+};
+
+
+char *gps_16_sym[]=
+{
+ "Dot","House","Fuel","Car","Fish","Boat","Anchor","Wreck",
+ "Exit","Skull","Flag","Camp","Circle-x","Deer","1st_aid","Back_track"
+};
+
+
+
+
+
+/* @func GPS_Fmt_Print_Time ********************************************
+**
+** Output Date/time
+**
+** @param [r] Time [time_t] unix-style time
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+void GPS_Fmt_Print_Time(time_t Time, FILE *outf)
+{
+ (void) fprintf(outf,"%s",ctime(&Time));
+ fflush(outf);
+
+ return;
+}
+
+
+
+/* @func GPS_Fmt_Print_Position ********************************************
+**
+** Output position
+**
+** @param [r] lat [double] latitude (deg)
+** @param [r] lon [double] longitude (deg)
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+void GPS_Fmt_Print_Position(double lat, double lon, FILE *outf)
+{
+ (void) fprintf(outf,"Latitude: %f Longitude %f\n",lat,lon);
+ fflush(outf);
+
+ return;
+}
+
+
+
+/* @func GPS_Fmt_Print_Pvt ********************************************
+**
+** Output pvt
+**
+** @param [r] pvt [GPS_PPvt_Data] pvt
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+void GPS_Fmt_Print_Pvt(GPS_PPvt_Data pvt, FILE *outf)
+{
+
+ (void) fprintf(outf,"Fix: ");
+ switch(pvt->fix)
+ {
+ case 0:
+ (void) fprintf(outf,"UNUSABLE\n\n");
+ break;
+ case 1:
+ (void) fprintf(outf,"INVALID \n\n");
+ break;
+ case 2:
+ (void) fprintf(outf,"2D \n\n");
+ break;
+ case 3:
+ (void) fprintf(outf,"3D \n\n");
+ break;
+ case 4:
+ (void) fprintf(outf,"2D-diff \n\n");
+ break;
+ case 5:
+ (void) fprintf(outf,"2D-diff \n\n");
+ break;
+ default:
+ (void) fprintf(stderr,"PVT: Unsupported Fix type\n");
+ break;
+ }
+
+ (void) fprintf(outf,"Altitude (WGS 84): %-20f \n",pvt->alt);
+ (void) fprintf(outf,"EPE: %-20f \n",pvt->epe);
+ (void) fprintf(outf,"EPE (hor only): %-20f \n",pvt->eph);
+ (void) fprintf(outf,"EPE (ver only): %-20f \n",pvt->epv);
+ (void) fprintf(outf,"Time of week: %-20d \n",(int)pvt->tow);
+ (void) fprintf(outf,"Latitude: %-20f \n",pvt->lat);
+ (void) fprintf(outf,"Longitude: %-20f \n",pvt->lon);
+ (void) fprintf(outf,"East velocity: %-20f \n",pvt->east);
+ (void) fprintf(outf,"North velocity: %-20f \n",pvt->north);
+ (void) fprintf(outf,"Upward velocity %-20f \n",pvt->up);
+ (void) fprintf(outf,"Height above MSL: %-20f \n",pvt->msl_hght+pvt->alt);
+ (void) fprintf(outf,"Leap seconds: %-20d \n",pvt->leap_scnds);
+ (void) fprintf(outf,"Week number days: %-20d \n",(int)pvt->wn_days);
+
+ fflush(outf);
+
+ return;
+}
+
+
+
+/* @func GPS_Fmt_Print_Almanac ********************************************
+**
+** Output almanac
+**
+** @param [r] alm [GPS_PAlmanac *] almanac array
+** @param [r] n [int32] number of almanac entries
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+void GPS_Fmt_Print_Almanac(GPS_PAlmanac *alm, int32 n, FILE *outf)
+{
+ int32 i;
+ int32 t;
+ int32 s;
+
+ /* Type 0 models require all 32 satellites to be sent */
+ t=32;
+ s=0;
+ if(n && alm[0]->svid!=0xff)
+ {
+ s=1;
+ t=n;
+ }
+ (void) fprintf(outf,"Almanac %d %d\n",(int)t,(int)s);
+
+
+ for(i=0;i<n;++i)
+ {
+ if(alm[i]->wn<0) continue;
+
+ if(alm[i]->svid == 0xff)
+ alm[i]->svid = i;
+ (void) fprintf(outf,"#\n#\n");
+ (void) fprintf(outf,"\tID: %d\n",
+ alm[i]->svid+1);
+ (void) fprintf(outf,"\tWeek number: %d\n",
+ alm[i]->wn);
+ (void) fprintf(outf,"\tAlmanac Data Reference Time: %f\n",
+ alm[i]->toa);
+ (void) fprintf(outf,"\tClock Correction Coeff (s): %f\n",
+ alm[i]->af0);
+ (void) fprintf(outf,"\tClock Correction Coeff (s/s): %f\n",
+ alm[i]->af1);
+ (void) fprintf(outf,"\tEccentricity: %f\n",
+ alm[i]->e);
+ (void) fprintf(outf,"\tSqrt of semi-major axis: %f\n",
+ alm[i]->sqrta);
+ (void) fprintf(outf,"\tMean Anomaly at Ref. Time: %f\n",
+ alm[i]->m0);
+ (void) fprintf(outf,"\tArgument of perigee: %f\n",
+ alm[i]->w);
+ (void) fprintf(outf,"\tRight ascension: %f\n",
+ alm[i]->omg0);
+ (void) fprintf(outf,"\tRate of right ascension: %f\n",
+ alm[i]->odot);
+ (void) fprintf(outf,"\tInclination angle: %f\n",
+ alm[i]->i);
+ (void) fprintf(outf,"\tHealth: %d\n",
+ alm[i]->hlth);
+ }
+
+
+ fflush(outf);
+
+ return;
+}
+
+
+
+/* @func GPS_Fmt_Print_Track ********************************************
+**
+** Output track log
+**
+** @param [r] trk [GPS_PTrack *] track array
+** @param [r] n [int32] number of track entries
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+void GPS_Fmt_Print_Track(GPS_PTrack *trk, int32 n, FILE *outf)
+{
+ int32 i;
+
+
+ switch(gps_trk_transfer)
+ {
+ case pA300:
+ break;
+ case pA301:
+ GPS_Fmt_Print_Track301(trk,n,outf);
+ return;
+ default:
+ GPS_Error("GPS_Fmt_Print_Track: Unknown protocol");
+ return;
+ }
+
+
+ (void) fprintf(outf,"Track log 300 %d\n#\n",(int)gps_trk_type);
+ (void) fprintf(outf,"Start\n#\n");
+
+ for(i=0;i<n;++i)
+ {
+ if(trk[i]->tnew)
+ (void) fprintf(outf,"#\nNew track\n#\n");
+
+ switch(gps_trk_type)
+ {
+ case pD300:
+ GPS_Fmt_Print_D300(trk[i],outf);
+ break;
+ case pD301:
+ GPS_Fmt_Print_D301(trk[i],outf);
+ break;
+ default:
+ break;
+ }
+ }
+
+ (void) fprintf(outf,"End\n#\n");
+ fflush(outf);
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_Fmt_Print_Track301 ***********************************
+**
+** Output track log
+**
+** @param [r] trk [GPS_PTrack *] track array
+** @param [r] n [int32] number of track entries
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+static void GPS_Fmt_Print_Track301(GPS_PTrack *trk, int32 n, FILE *outf)
+{
+ int32 i;
+
+ if(!n)
+ return;
+
+ (void) fprintf(outf,"Track log 301 %d\n#\n",(int)gps_trk_type);
+ (void) fprintf(outf,"Start\n#\n");
+
+ for(i=0;i<n;++i)
+ {
+ if(trk[i]->ishdr)
+ {
+ (void) fprintf(outf,"Header\n");
+ (void) fprintf(outf,"\tIdent: %s\n",trk[i]->trk_ident);
+ (void) fprintf(outf,"\tDisplay: %d\n",(int)trk[i]->dspl);
+ (void) fprintf(outf,"\tColour: %d\n#\n",
+ (int)trk[i]->colour);
+ continue;
+ }
+
+ if(trk[i]->tnew)
+ (void) fprintf(outf,"#\nNew track\n#\n");
+
+ switch(gps_trk_type)
+ {
+ case pD300:
+ GPS_Fmt_Print_D300(trk[i],outf);
+ break;
+ case pD301:
+ GPS_Fmt_Print_D301(trk[i],outf);
+ break;
+ default:
+ break;
+ }
+ }
+
+ (void) fprintf(outf,"End\n#\n");
+ fflush(outf);
+
+ return;
+}
+
+
+/* @funcstatic GPS_Fmt_Print_D300 ****************************************
+**
+** Output track log
+**
+** @param [r] trk [GPS_PTrack *] track array
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+static void GPS_Fmt_Print_D300(GPS_PTrack trk, FILE *outf)
+{
+ (void) fprintf(outf,"\tLatitude: %f\n",trk->lat);
+ (void) fprintf(outf,"\tLongitude: %f\n",trk->lon);
+ if(trk->Time)
+ (void) fprintf(outf,"\tTime: %s\n",ctime(&trk->Time));
+ else
+ (void) fprintf(outf,"\tTime: Computer\n\n");
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_Fmt_Print_D301 ****************************************
+**
+** Output track log
+**
+** @param [r] trk [GPS_PTrack *] track array
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+static void GPS_Fmt_Print_D301(GPS_PTrack trk, FILE *outf)
+{
+ (void) fprintf(outf,"\tLatitude: %f\n",trk->lat);
+ (void) fprintf(outf,"\tLongitude: %f\n",trk->lon);
+ if(trk->Time)
+ (void) fprintf(outf,"\tTime: %s",ctime(&trk->Time));
+ else
+ (void) fprintf(outf,"\tTime: Computer\n");
+ (void) fprintf(outf,"\tAltitude: %f\n",trk->alt);
+ (void) fprintf(outf,"\tDepth: %f\n\n",trk->dpth);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Fmt_Print_Waypoint *****************************************
+**
+** Output waypoints
+**
+** @param [r] way [GPS_PWay *] waypoint array
+** @param [r] n [int32] number of waypoint entries
+** @param [w] outf [FILE *] output stream
+**
+** @return [int32] success
+************************************************************************/
+
+int32 GPS_Fmt_Print_Waypoint(GPS_PWay *way, int32 n, FILE *outf)
+{
+ int32 i;
+
+
+ if(!n)
+ return 1;
+
+ (void) fprintf(outf,"Waypoints Type: %d\n#\nStart\n#\n",
+ (int)way[0]->prot);
+
+
+ for(i=0;i<n;++i)
+ {
+ switch(way[i]->prot)
+ {
+ case 100:
+ GPS_Fmt_Print_Way100(way[i],outf);
+ break;
+ case 101:
+ GPS_Fmt_Print_Way101(way[i],outf);
+ break;
+ case 102:
+ GPS_Fmt_Print_Way102(way[i],outf);
+ break;
+ case 103:
+ GPS_Fmt_Print_Way103(way[i],outf);
+ break;
+ case 104:
+ GPS_Fmt_Print_Way104(way[i],outf);
+ break;
+ case 105:
+ GPS_Fmt_Print_Way105(way[i],outf);
+ break;
+ case 106:
+ GPS_Fmt_Print_Way106(way[i],outf);
+ break;
+ case 107:
+ GPS_Fmt_Print_Way107(way[i],outf);
+ break;
+ case 108:
+ GPS_Fmt_Print_Way108(way[i],outf);
+ break;
+ case 150:
+ GPS_Fmt_Print_Way150(way[i],outf);
+ break;
+ case 151:
+ GPS_Fmt_Print_Way151(way[i],outf);
+ break;
+ case 152:
+ GPS_Fmt_Print_Way152(way[i],outf);
+ break;
+ case 154:
+ GPS_Fmt_Print_Way154(way[i],outf);
+ break;
+ case 155:
+ GPS_Fmt_Print_Way155(way[i],outf);
+ break;
+ default:
+ GPS_Error("Print_Waypoint: Unknown waypoint protocol");
+ return PROTOCOL_ERROR;
+ }
+ (void) fprintf(outf,"#\n");
+ }
+ (void) fprintf(outf,"End\n#\n");
+
+ return 1;
+}
+
+
+
+/* @func GPS_Fmt_Print_Proximity *****************************************
+**
+** Output proximity
+**
+** @param [r] way [GPS_PWay *] waypoint array
+** @param [r] n [int32] number of waypoint entries
+** @param [w] outf [FILE *] output stream
+**
+** @return [int32] success
+************************************************************************/
+
+int32 GPS_Fmt_Print_Proximity(GPS_PWay *way, int32 n, FILE *outf)
+{
+ int32 i;
+
+
+ if(!n)
+ return 1;
+
+ (void) fprintf(outf,"Waypoints Type: %d\n#\nStart\n#\n",
+ (int)way[0]->prot);
+
+
+ for(i=0;i<n;++i)
+ {
+ switch(way[i]->prot)
+ {
+ case 400:
+ GPS_Fmt_Print_Way100(way[i],outf);
+ break;
+ case 101:
+ GPS_Fmt_Print_Way101(way[i],outf);
+ break;
+ case 102:
+ GPS_Fmt_Print_Way102(way[i],outf);
+ break;
+ case 403:
+ GPS_Fmt_Print_Way103(way[i],outf);
+ break;
+ case 104:
+ GPS_Fmt_Print_Way104(way[i],outf);
+ break;
+ case 105:
+ GPS_Fmt_Print_Way105(way[i],outf);
+ break;
+ case 106:
+ GPS_Fmt_Print_Way106(way[i],outf);
+ break;
+ case 107:
+ GPS_Fmt_Print_Way107(way[i],outf);
+ break;
+ case 108:
+ GPS_Fmt_Print_Way108(way[i],outf);
+ break;
+ case 450:
+ GPS_Fmt_Print_Way150(way[i],outf);
+ (void) fprintf(outf,"\tPindex: %d\n",(int)way[i]->idx);
+ break;
+ case 151:
+ GPS_Fmt_Print_Way151(way[i],outf);
+ break;
+ case 152:
+ GPS_Fmt_Print_Way152(way[i],outf);
+ break;
+ case 154:
+ GPS_Fmt_Print_Way154(way[i],outf);
+ break;
+ case 155:
+ GPS_Fmt_Print_Way155(way[i],outf);
+ break;
+ default:
+ GPS_Error("Print_Proximity: Unknown proximity protocol");
+ return PROTOCOL_ERROR;
+ }
+ (void) fprintf(outf,"\tDistance: %f\n",way[i]->dst);
+ (void) fprintf(outf,"#\n");
+ }
+ (void) fprintf(outf,"End\n#\n");
+
+ return 1;
+}
+
+
+
+
+/* @funcstatic GPS_Fmt_Print_Way100 *************************************
+**
+** Output waypoint D100
+**
+** @param [r] way [GPS_PWay] waypoint
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+static void GPS_Fmt_Print_Way100(GPS_PWay way, FILE *outf)
+{
+
+ (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident);
+ (void) fprintf(outf,"\tLatitude: %f\n",way->lat);
+ (void) fprintf(outf,"\tLongitude: %f\n",way->lon);
+ (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt);
+
+ return;
+}
+
+
+/* @funcstatic GPS_Fmt_Print_Way101 ************************************
+**
+** Output waypoint D101
+**
+** @param [r] way [GPS_PWay] waypoint
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+static void GPS_Fmt_Print_Way101(GPS_PWay way, FILE *outf)
+{
+
+ if(way->smbl > 176) way->smbl=36;
+
+ (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident);
+ (void) fprintf(outf,"\tLatitude: %f\n",way->lat);
+ (void) fprintf(outf,"\tLongitude: %f\n",way->lon);
+ (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt);
+ (void) fprintf(outf,"\tSymbol: %d [%s]\n",(int)way->smbl,
+ gps_marine_sym[way->smbl]);
+
+ return;
+}
+
+
+/* @funcstatic GPS_Fmt_Print_Way102 *************************************
+**
+** Output waypoint D102
+**
+** @param [r] way [GPS_PWay] waypoint
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+static void GPS_Fmt_Print_Way102(GPS_PWay way, FILE *outf)
+{
+ char **p;
+ int32 x;
+
+ if(way->smbl < 8192)
+ {
+ p = gps_marine_sym;
+ x = 0;
+ }
+ else if(way->smbl < 16384)
+ {
+ p = gps_land_sym;
+ x = 8192;
+ }
+ else
+ {
+ p = gps_aviation_sym;
+ x = 16384;
+ }
+
+
+ (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident);
+ (void) fprintf(outf,"\tLatitude: %f\n",way->lat);
+ (void) fprintf(outf,"\tLongitude: %f\n",way->lon);
+ (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt);
+ (void) fprintf(outf,"\tSymbol: %d [%s]\n",(int)way->smbl,
+ p[way->smbl-x]);
+
+ return;
+}
+
+
+/* @funcstatic GPS_Fmt_Print_Way103 ************************************
+**
+** Output waypoint D103
+**
+** @param [r] way [GPS_PWay] waypoint
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+static void GPS_Fmt_Print_Way103(GPS_PWay way, FILE *outf)
+{
+ static char *dspl[]=
+ {
+ "SW","S","SC"
+ };
+
+ (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident);
+ (void) fprintf(outf,"\tLatitude: %f\n",way->lat);
+ (void) fprintf(outf,"\tLongitude: %f\n",way->lon);
+ (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt);
+ (void) fprintf(outf,"\tSymbol: %-6d [%s]\n",(int)way->smbl,
+ gps_16_sym[way->smbl]);
+ (void) fprintf(outf,"\tDisplay: %-6d [%s]\n",(int)way->dspl,
+ dspl[way->dspl]);
+
+ return;
+}
+
+
+/* @funcstatic GPS_Fmt_Print_Way104 *************************************
+**
+** Output waypoint D104
+**
+** @param [r] way [GPS_PWay] waypoint
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+static void GPS_Fmt_Print_Way104(GPS_PWay way, FILE *outf)
+{
+ static char *dspl[]=
+ {
+ "S","S","","SW","","SC"
+ };
+ char **p;
+ int32 x;
+
+ if(way->smbl < 8192)
+ {
+ p = gps_marine_sym;
+ x = 0;
+ }
+ else if(way->smbl < 16384)
+ {
+ p = gps_land_sym;
+ x = 8192;
+ }
+ else
+ {
+ p = gps_aviation_sym;
+ x = 16384;
+ }
+
+ (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident);
+ (void) fprintf(outf,"\tLatitude: %f\n",way->lat);
+ (void) fprintf(outf,"\tLongitude: %f\n",way->lon);
+ (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt);
+ (void) fprintf(outf,"\tSymbol: %-6d [%s]\n",(int)way->smbl,
+ p[way->smbl-x]);
+ (void) fprintf(outf,"\tDisplay: %-6d [%s]\n",(int)way->dspl,
+ dspl[way->dspl]);
+
+ return;
+}
+
+
+/* @funcstatic GPS_Fmt_Print_Way105 ************************************
+**
+** Output waypoint D105
+**
+** @param [r] way [GPS_PWay] waypoint
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+static void GPS_Fmt_Print_Way105(GPS_PWay way, FILE *outf)
+{
+ char **p;
+ int32 x;
+
+ if(way->smbl < 8192)
+ {
+ p = gps_marine_sym;
+ x = 0;
+ }
+ else if(way->smbl < 16384)
+ {
+ p = gps_land_sym;
+ x = 8192;
+ }
+ else
+ {
+ p = gps_aviation_sym;
+ x = 16384;
+ }
+
+ (void) fprintf(outf,"\tLatitude: %f\n",way->lat);
+ (void) fprintf(outf,"\tLongitude: %f\n",way->lon);
+ (void) fprintf(outf,"\tSymbol: %-6d [%s]\n",(int)way->smbl,
+ p[way->smbl-x]);
+ (void) fprintf(outf,"\tWpt_ident %s\n",way->wpt_ident);
+
+ return;
+}
+
+
+/* @funcstatic GPS_Fmt_Print_Way106 *************************************
+**
+** Output waypoint D106
+**
+** @param [r] way [GPS_PWay] waypoint
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+static void GPS_Fmt_Print_Way106(GPS_PWay way, FILE *outf)
+{
+ char **p;
+ int32 x;
+
+ if(way->smbl < 8192)
+ {
+ p = gps_marine_sym;
+ x = 0;
+ }
+ else if(way->smbl < 16384)
+ {
+ p = gps_land_sym;
+ x = 8192;
+ }
+ else
+ {
+ p = gps_aviation_sym;
+ x = 16384;
+ }
+
+ if(!way->wpt_class)
+ {
+ (void) fprintf(outf,"\tClass: %d [User]\n",way->wpt_class);
+ (void) fprintf(outf,"\tSubclass: %d [%-13.13s]\n",
+ way->wpt_class,way->subclass);
+ (void) fprintf(outf,"\tSubclass:\n");
+ }
+ else
+ {
+ (void) fprintf(outf,"\tClass: %d [Non-user]\n",
+ way->wpt_class);
+ (void) fprintf(outf,"\tSubclass: %d [%13.13s]\n",
+ way->wpt_class,
+ way->subclass);
+ }
+ (void) fprintf(outf,"\tLatitude: %f\n",way->lat);
+ (void) fprintf(outf,"\tLongitude: %f\n",way->lon);
+ (void) fprintf(outf,"\tSymbol: %-6d [%s]\n",(int)way->smbl,
+ p[way->smbl-x]);
+ (void) fprintf(outf,"\tWpt_ident %s\n",way->wpt_ident);
+ (void) fprintf(outf,"\tLnk_ident %s\n",way->lnk_ident);
+
+ return;
+}
+
+
+/* @funcstatic GPS_Fmt_Print_Way107 ************************************
+**
+** Output waypoint D107
+**
+** @param [r] way [GPS_PWay] waypoint
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+static void GPS_Fmt_Print_Way107(GPS_PWay way, FILE *outf)
+{
+ static char *dspl[]=
+ {
+ "SW","S","SC"
+ };
+ static char *col[]=
+ {
+ "Default","Red","Green","Blue"
+ };
+
+
+ (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident);
+ (void) fprintf(outf,"\tLatitude: %f\n",way->lat);
+ (void) fprintf(outf,"\tLongitude: %f\n",way->lon);
+ (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt);
+ (void) fprintf(outf,"\tSymbol: %-6d [%s]\n",(int)way->smbl,
+ gps_16_sym[way->smbl]);
+ (void) fprintf(outf,"\tDisplay: %-6d [%s]\n",(int)way->dspl,
+ dspl[way->dspl]);
+ (void) fprintf(outf,"\tColour: %-6d [%s]\n",(int)way->colour,
+ col[way->colour]);
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_Fmt_Print_Way108 ************************************
+**
+** Output waypoint D108
+**
+** @param [r] way [GPS_PWay] waypoint
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+static void GPS_Fmt_Print_Way108(GPS_PWay way, FILE *outf)
+{
+ char **p;
+ int32 x;
+
+ static char *dspl[]=
+ {
+ "SW","S","SC"
+ };
+
+ static char *col[]=
+ {
+ "Black","Dark_Red","Dark_Green","Dark_Yellow","Dark_Blue",
+ "Dark_Magenta","Dark_Cyan","Light_Grey","Dark_Grey","Red","Green",
+ "Yellow","Blue","Magenta","Cyan","White"
+ };
+
+
+ if(way->smbl < 8192)
+ {
+ p = gps_marine_sym;
+ x = 0;
+ }
+ else if(way->smbl < 16384)
+ {
+ p = gps_land_sym;
+ x = 8192;
+ }
+ else
+ {
+ p = gps_aviation_sym;
+ x = 16384;
+ }
+
+ (void) fprintf(outf,"\tIdent: %s\n",way->ident);
+ (void) fprintf(outf,"\tLatitude: %f\n",way->lat);
+ (void) fprintf(outf,"\tLongitude: %f\n",way->lon);
+ if(way->colour==0xff)
+ (void) fprintf(outf,"\tColour: 255 [Default]\n");
+ else
+ (void) fprintf(outf,"\tColour: %-6d [%s]\n",(int)way->colour,
+ col[way->colour]);
+ (void) fprintf(outf,"\tDisplay: %-6d [%s]\n",(int)way->dspl,
+ dspl[way->dspl]);
+ (void) fprintf(outf,"\tSymbol: %-6d [%s]\n",(int)way->smbl,
+ p[way->smbl-x]);
+ (void) fprintf(outf,"\tAltitude: %d\n",(int)way->alt);
+ (void) fprintf(outf,"\tDepth: %f\n",way->dpth);
+ (void) fprintf(outf,"\tState: %-2.2s\n",way->state);
+ (void) fprintf(outf,"\tCountry: %-2.2s\n",way->cc);
+ (void) fprintf(outf,"\tClass: %d\n",way->wpt_class);
+ if(way->wpt_class>=0x80 && way->wpt_class<=0x85)
+ (void) fprintf(outf,"\tSubclass: %18.18s\n",way->subclass);
+ if(!way->wpt_class)
+ (void) fprintf(outf,"\tComment: %s\n",way->cmnt);
+ if(way->wpt_class>=0x40 && way->wpt_class<=0x46)
+ {
+ (void) fprintf(outf,"\tFacility: %s\n",way->facility);
+ (void) fprintf(outf,"\tCity: %s\n",way->city);
+ }
+ if(way->wpt_class==0x83)
+ (void) fprintf(outf,"\tAddress: %s\n",way->addr);
+ if(way->wpt_class==0x82)
+ (void) fprintf(outf,"\tCross Road: %s\n",way->cross_road);
+
+
+ return;
+}
+
+
+/* @funcstatic GPS_Fmt_Print_Way150 *************************************
+**
+** Output waypoint D150
+**
+** @param [r] way [GPS_PWay] waypoint
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+static void GPS_Fmt_Print_Way150(GPS_PWay way, FILE *outf)
+{
+
+ (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident);
+ (void) fprintf(outf,"\tLatitude: %f\n",way->lat);
+ (void) fprintf(outf,"\tLongitude: %f\n",way->lon);
+ (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt);
+ (void) fprintf(outf,"\tClass: %d\n",way->wpt_class);
+ if(way->wpt_class!=4)
+ {
+ (void) fprintf(outf,"\tCountry: %-2.2s\n",way->cc);
+ (void) fprintf(outf,"\tCity: %-24.24s\n",way->city);
+ (void) fprintf(outf,"\tState: %-2.2s\n",way->state);
+ (void) fprintf(outf,"\tName: %-30.30s\n",way->name);
+ }
+ if(!way->wpt_class)
+ (void) fprintf(outf,"\tAltitude: %d\n",(int)way->alt);
+
+ return;
+}
+
+
+/* @funcstatic GPS_Fmt_Print_Way151 ************************************
+**
+** Output waypoint D151
+**
+** @param [r] way [GPS_PWay] waypoint
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+static void GPS_Fmt_Print_Way151(GPS_PWay way, FILE *outf)
+{
+
+ (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident);
+ (void) fprintf(outf,"\tLatitude: %f\n",way->lat);
+ (void) fprintf(outf,"\tLongitude: %f\n",way->lon);
+ (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt);
+ (void) fprintf(outf,"\tClass: %d\n",way->wpt_class);
+ if(way->wpt_class!=2)
+ {
+ (void) fprintf(outf,"\tCountry: %-2.2s\n",way->cc);
+ (void) fprintf(outf,"\tCity: %-24.24s\n",way->city);
+ (void) fprintf(outf,"\tState: %-2.2s\n",way->state);
+ (void) fprintf(outf,"\tName: %-30.30s\n",way->name);
+ }
+ if(!way->wpt_class)
+ (void) fprintf(outf,"\tAltitude: %d\n",(int)way->alt);
+
+ return;
+}
+
+
+
+/* @funcstatic GPS_Fmt_Print_Way152 ************************************
+**
+** Output waypoint D152
+**
+** @param [r] way [GPS_PWay] waypoint
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+static void GPS_Fmt_Print_Way152(GPS_PWay way, FILE *outf)
+{
+
+ (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident);
+ (void) fprintf(outf,"\tLatitude: %f\n",way->lat);
+ (void) fprintf(outf,"\tLongitude: %f\n",way->lon);
+ (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt);
+ (void) fprintf(outf,"\tClass: %d\n",way->wpt_class);
+ if(way->wpt_class!=4)
+ {
+ (void) fprintf(outf,"\tCountry: %-2.2s\n",way->cc);
+ (void) fprintf(outf,"\tCity: %-24.24s\n",way->city);
+ (void) fprintf(outf,"\tState: %-2.2s\n",way->state);
+ (void) fprintf(outf,"\tName: %-30.30s\n",way->name);
+ }
+ if(!way->wpt_class)
+ (void) fprintf(outf,"\tAltitude: %d\n",(int)way->alt);
+
+ return;
+}
+
+
+/* @funcstatic GPS_Fmt_Print_Way154 ************************************
+**
+** Output waypoint D154
+**
+** @param [r] way [GPS_PWay] waypoint
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+static void GPS_Fmt_Print_Way154(GPS_PWay way, FILE *outf)
+{
+ char **p;
+ int32 x;
+
+ if(way->smbl < 8192)
+ {
+ p = gps_marine_sym;
+ x = 0;
+ }
+ else if(way->smbl < 16384)
+ {
+ p = gps_land_sym;
+ x = 8192;
+ }
+ else
+ {
+ p = gps_aviation_sym;
+ x = 16384;
+ }
+
+
+ (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident);
+ (void) fprintf(outf,"\tLatitude: %f\n",way->lat);
+ (void) fprintf(outf,"\tLongitude: %f\n",way->lon);
+ (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt);
+ (void) fprintf(outf,"\tSymbol: %-6d [%s]\n",(int)way->smbl,
+ p[way->smbl-x]);
+ (void) fprintf(outf,"\tClass: %d\n",way->wpt_class);
+ if(way->wpt_class!=4 && way->wpt_class!=8)
+ {
+ (void) fprintf(outf,"\tCountry: %-2.2s\n",way->cc);
+ (void) fprintf(outf,"\tCity: %-24.24s\n",way->city);
+ (void) fprintf(outf,"\tState: %-2.2s\n",way->state);
+ (void) fprintf(outf,"\tName: %-30.30s\n",way->name);
+ }
+ if(!way->wpt_class)
+ (void) fprintf(outf,"\tAltitude: %d\n",(int)way->alt);
+
+ return;
+}
+
+
+/* @funcstatic GPS_Fmt_Print_Way155 *************************************
+**
+** Output waypoint D155
+**
+** @param [r] way [GPS_PWay] waypoint
+** @param [w] outf [FILE *] output stream
+**
+** @return [void]
+************************************************************************/
+
+static void GPS_Fmt_Print_Way155(GPS_PWay way, FILE *outf)
+{
+ static char *dspl[]=
+ {
+ "","S","","SW","","SC"
+ };
+
+ char **p;
+ int32 x;
+
+ if(way->smbl < 8192)
+ {
+ p = gps_marine_sym;
+ x = 0;
+ }
+ else if(way->smbl < 16384)
+ {
+ p = gps_land_sym;
+ x = 8192;
+ }
+ else
+ {
+ p = gps_aviation_sym;
+ x = 16384;
+ }
+
+
+ (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident);
+ (void) fprintf(outf,"\tLatitude: %f\n",way->lat);
+ (void) fprintf(outf,"\tLongitude: %f\n",way->lon);
+ (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt);
+ (void) fprintf(outf,"\tSymbol: %-6d [%s]\n",(int)way->smbl,
+ p[way->smbl-x]);
+ (void) fprintf(outf,"\tDisplay: %-6d [%s]\n",(int)way->dspl,
+ dspl[way->dspl]);
+ (void) fprintf(outf,"\tClass: %d\n",way->wpt_class);
+ if(way->wpt_class!=4 && way->wpt_class!=8)
+ {
+ (void) fprintf(outf,"\tCountry: %-2.2s\n",way->cc);
+ (void) fprintf(outf,"\tCity: %-24.24s\n",way->city);
+ (void) fprintf(outf,"\tState: %-2.2s\n",way->state);
+ (void) fprintf(outf,"\tName: %-30.30s\n",way->name);
+ }
+ if(!way->wpt_class)
+ (void) fprintf(outf,"\tAltitude: %d\n",(int)way->alt);
+
+ return;
+}
+
+
+
+/* @func GPS_Fmt_Print_Route *****************************************
+**
+** Output route(s)
+**
+** @param [r] way [GPS_PWay *] waypoint array
+** @param [r] n [int32] number of waypoint entries
+** @param [w] outf [FILE *] output stream
+**
+** @return [int32] success
+************************************************************************/
+
+int32 GPS_Fmt_Print_Route(GPS_PWay *way, int32 n, FILE *outf)
+{
+ int32 i;
+ int32 first;
+
+ if(!n)
+ return 1;
+
+
+ switch(gps_route_transfer)
+ {
+ case pA200:
+ break;
+ case pA201:
+ return GPS_Fmt_Print_Route201(way,n,outf);
+ default:
+ GPS_Error("GPS_Fmt_Print_Route: Unknown protocol");
+ return PROTOCOL_ERROR;
+ }
+
+
+ (void) fprintf(outf,"Route log 200 %d\n#\n",(int)gps_rte_type);
+ (void) fprintf(outf,"Start\n#\n");
+
+
+
+ first = 1;
+
+ for(i=0;i<n;++i)
+ {
+ if(way[i]->isrte)
+ {
+ if(!first)
+ (void) fprintf(outf,"End\n#\n");
+ (void) fprintf(outf,"Route Type: %d ",(int)way[i]->rte_prot);
+ first=0;
+
+ switch(way[i]->rte_prot)
+ {
+ case 200:
+ (void) fprintf(outf,"Number: %d",way[i]->rte_num);
+ break;
+ case 201:
+ (void) fprintf(outf,"Number: %d Comment: %-20.20s",
+ way[i]->rte_num,way[i]->rte_cmnt);
+ break;
+ case 202:
+ (void) fprintf(outf,"Comment: %s",way[i]->rte_ident);
+ break;
+ default:
+ GPS_Error("Print_Route: Unknown route protocol");
+ return PROTOCOL_ERROR;
+ }
+ (void) fprintf(outf,"\n#\n");
+ (void) fprintf(outf,"Waypoints Type: %d\n#\nStart\n#\n"
+ ,(int)way[i]->prot);
+ continue;
+ }
+
+ switch(way[i]->prot)
+ {
+ case 100:
+ GPS_Fmt_Print_Way100(way[i],outf);
+ break;
+ case 101:
+ GPS_Fmt_Print_Way101(way[i],outf);
+ break;
+ case 102:
+ GPS_Fmt_Print_Way102(way[i],outf);
+ break;
+ case 103:
+ GPS_Fmt_Print_Way103(way[i],outf);
+ break;
+ case 104:
+ GPS_Fmt_Print_Way104(way[i],outf);
+ break;
+ case 105:
+ GPS_Fmt_Print_Way105(way[i],outf);
+ break;
+ case 106:
+ GPS_Fmt_Print_Way106(way[i],outf);
+ break;
+ case 107:
+ GPS_Fmt_Print_Way107(way[i],outf);
+ break;
+ case 108:
+ GPS_Fmt_Print_Way108(way[i],outf);
+ break;
+ case 150:
+ GPS_Fmt_Print_Way150(way[i],outf);
+ break;
+ case 151:
+ GPS_Fmt_Print_Way151(way[i],outf);
+ break;
+ case 152:
+ GPS_Fmt_Print_Way152(way[i],outf);
+ break;
+ case 154:
+ GPS_Fmt_Print_Way154(way[i],outf);
+ break;
+ case 155:
+ GPS_Fmt_Print_Way155(way[i],outf);
+ break;
+ default:
+ GPS_Error("Print_Route: Unknown waypoint protocol");
+ return PROTOCOL_ERROR;
+ }
+ (void) fprintf(outf,"#\n");
+ }
+ (void) fprintf(outf,"End\n#\n");
+
+ return 1;
+}
+
+
+
+/* @funcstatic GPS_Fmt_Print_Route201 ***********************************
+**
+** Output route(s)
+**
+** @param [r] way [GPS_PWay *] waypoint array
+** @param [r] n [int32] number of waypoint entries
+** @param [w] outf [FILE *] output stream
+**
+** @return [int32] success
+************************************************************************/
+
+static int32 GPS_Fmt_Print_Route201(GPS_PWay *way, int32 n, FILE *outf)
+{
+ int32 i;
+ int32 first;
+
+ if(!n)
+ return 1;
+
+
+ (void) fprintf(outf,"Route log 201 %d\n#\n",(int)gps_rte_link_type);
+ (void) fprintf(outf,"Start\n#\n");
+
+
+ first = 1;
+
+ for(i=0;i<n;++i)
+ {
+ if(way[i]->isrte)
+ {
+ if(!first)
+ (void) fprintf(outf,"End\n#\n");
+ (void) fprintf(outf,"Route Type: %d ",(int)way[i]->rte_prot);
+ first=0;
+
+ switch(way[i]->rte_prot)
+ {
+ case 200:
+ (void) fprintf(outf,"Number: %d",way[i]->rte_num);
+ break;
+ case 201:
+ (void) fprintf(outf,"Number: %d Comment: %-20.20s",
+ way[i]->rte_num,way[i]->rte_cmnt);
+ break;
+ case 202:
+ (void) fprintf(outf,"Comment: %s",way[i]->rte_ident);
+ break;
+ default:
+ GPS_Error("Print_Route: Unknown route protocol");
+ return PROTOCOL_ERROR;
+ }
+ (void) fprintf(outf,"\n#\n");
+ (void) fprintf(outf,"Waypoints Type: %d\n#\n"
+ ,(int)way[i]->prot);
+ continue;
+ }
+
+
+ if(way[i]->islink)
+ {
+ (void) fprintf(outf,"\tLink Class: %d\n",
+ (int)way[i]->rte_link_class);
+ if(!(way[i]->rte_link_class==3 || way[i]->rte_link_class==0xff))
+ (void) fprintf(outf,"\tLink Subclass: %-18.18s\n",
+ way[i]->rte_link_subclass);
+ (void) fprintf(outf,"\tLink Ident: %s\n#\n",
+ way[i]->rte_link_ident);
+ continue;
+ }
+
+
+ switch(way[i]->prot)
+ {
+ case 100:
+ GPS_Fmt_Print_Way100(way[i],outf);
+ break;
+ case 101:
+ GPS_Fmt_Print_Way101(way[i],outf);
+ break;
+ case 102:
+ GPS_Fmt_Print_Way102(way[i],outf);
+ break;
+ case 103:
+ GPS_Fmt_Print_Way103(way[i],outf);
+ break;
+ case 104:
+ GPS_Fmt_Print_Way104(way[i],outf);
+ break;
+ case 105:
+ GPS_Fmt_Print_Way105(way[i],outf);
+ break;
+ case 106:
+ GPS_Fmt_Print_Way106(way[i],outf);
+ break;
+ case 107:
+ GPS_Fmt_Print_Way107(way[i],outf);
+ break;
+ case 108:
+ GPS_Fmt_Print_Way108(way[i],outf);
+ break;
+ case 150:
+ GPS_Fmt_Print_Way150(way[i],outf);
+ break;
+ case 151:
+ GPS_Fmt_Print_Way151(way[i],outf);
+ break;
+ case 152:
+ GPS_Fmt_Print_Way152(way[i],outf);
+ break;
+ case 154:
+ GPS_Fmt_Print_Way154(way[i],outf);
+ break;
+ case 155:
+ GPS_Fmt_Print_Way155(way[i],outf);
+ break;
+ default:
+ GPS_Error("Print_Route: Unknown waypoint protocol");
+ return PROTOCOL_ERROR;
+ }
+ (void) fprintf(outf,"#\n");
+ }
+ (void) fprintf(outf,"End\n");
+
+ return 1;
+}
--- /dev/null
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef gpsfmt_h
+#define gpsfmt_h
+
+
+#include "gps.h"
+#include <stdio.h>
+#include <time.h>
+
+void GPS_Fmt_Print_Time(time_t Time, FILE *outf);
+void GPS_Fmt_Print_Position(double lat, double lon, FILE *outf);
+void GPS_Fmt_Print_Pvt(GPS_PPvt_Data pvt, FILE *outf);
+void GPS_Fmt_Print_Almanac(GPS_PAlmanac *alm, int32 n, FILE *outf);
+void GPS_Fmt_Print_Track(GPS_PTrack *trk, int32 n, FILE *outf);
+int32 GPS_Fmt_Print_Waypoint(GPS_PWay *way, int32 n, FILE *outf);
+int32 GPS_Fmt_Print_Proximity(GPS_PWay *way, int32 n, FILE *outf);
+int32 GPS_Fmt_Print_Route(GPS_PWay *way, int32 n, FILE *outf);
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null
+/********************************************************************
+** @source JEEPS input functions
+**
+** @author Copyright (C) 1999 Alan Bleasby
+** @version 1.0
+** @modified Dec 28 1999 Alan Bleasby. First version
+** @@
+**
+** This library is free software; you can redistribute it and/or
+** modify it under the terms of the GNU Library General Public
+** License as published by the Free Software Foundation; either
+** version 2 of the License, or (at your option) any later version.
+**
+** This library is distributed in the hope that it will be useful,
+** but WITHOUT ANY WARRANTY; without even the implied warranty of
+** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+** Library General Public License for more details.
+**
+** You should have received a copy of the GNU Library General Public
+** License along with this library; if not, write to the
+** Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+** Boston, MA 02111-1307, USA.
+********************************************************************/
+#include "gps.h"
+#include <stdio.h>
+#include <errno.h>
+#include <string.h>
+#include <stdlib.h>
+
+
+static int32 GPS_Input_Load_String(char *t, int32 n, char *s);
+static int32 GPS_Input_Load_Strnull(char *t, char *s);
+static int32 GPS_Input_Read_Line(char *s, FILE *inf);
+
+static int32 GPS_Input_Get_D100(GPS_PWay *way, FILE *inf);
+static int32 GPS_Input_Get_D101(GPS_PWay *way, FILE *inf);
+static int32 GPS_Input_Get_D102(GPS_PWay *way, FILE *inf);
+static int32 GPS_Input_Get_D103(GPS_PWay *way, FILE *inf);
+static int32 GPS_Input_Get_D104(GPS_PWay *way, FILE *inf);
+static int32 GPS_Input_Get_D105(GPS_PWay *way, FILE *inf);
+static int32 GPS_Input_Get_D106(GPS_PWay *way, FILE *inf);
+static int32 GPS_Input_Get_D107(GPS_PWay *way, FILE *inf);
+static int32 GPS_Input_Get_D108(GPS_PWay *way, FILE *inf);
+static int32 GPS_Input_Get_D150(GPS_PWay *way, FILE *inf);
+static int32 GPS_Input_Get_D151(GPS_PWay *way, FILE *inf);
+static int32 GPS_Input_Get_D152(GPS_PWay *way, FILE *inf);
+static int32 GPS_Input_Get_D154(GPS_PWay *way, FILE *inf);
+static int32 GPS_Input_Get_D155(GPS_PWay *way, FILE *inf);
+
+static int32 GPS_Input_Get_Track301(GPS_PTrack **trk, FILE *inf, int32 type,
+ int32 n);
+static int32 GPS_Input_Get_D300(GPS_PTrack *trk, FILE *inf, char *s);
+static int32 GPS_Input_Get_D301(GPS_PTrack *trk, FILE *inf, char *s);
+
+static int32 GPS_Input_Get_Route201(GPS_PWay **way, FILE *inf);
+
+
+/* @funcstatic GPS_Input_Load_String ***********************************
+**
+** Load a GPS char type from an input line
+** Remove trailing newline
+**
+** @param [w] t [char *] string to load
+** @param [r] n [int32] maximum type length
+** @param [r] s [char *] source line
+**
+** @return [int32] success
+************************************************************************/
+static int32 GPS_Input_Load_String(char *t, int32 n, char *s)
+{
+ char *p;
+ char *q;
+
+ int32 len;
+ int32 i;
+
+ gps_errno = INPUT_ERROR;
+
+ p=s;
+ if(!(p=strchr(p,':')))
+ return gps_errno;
+ ++p;
+ while(*p && (*p==' ' || *p=='\t')) ++p;
+ if(!*p)
+ return 0;
+
+ len = strlen(p);
+ q = p+len-1;
+ while(*q==' ' || *q=='\t') --q;
+ len = q-p+1;
+
+ if(q-p+1 > n)
+ {
+ len = n;
+ p[n]='\0';
+ }
+ for(i=0;i<len;++i) t[i]=*p++;
+
+ return 1;
+}
+
+
+
+/* @funcstatic GPS_Input_Load_Strnull **********************************
+**
+** Load a GPS variable length string type from an input line
+** Remove trailing newline
+**
+** @param [w] t [char *] string to load
+** @param [r] s [char *] source line
+**
+** @return [int32] success
+************************************************************************/
+static int32 GPS_Input_Load_Strnull(char *t, char *s)
+{
+ char *p;
+ char *q;
+
+ gps_errno = INPUT_ERROR;
+
+ p=s;
+ if(!(p=strchr(p,':')))
+ return gps_errno;
+ ++p;
+ while(*p && (*p==' ' || *p=='\t')) ++p;
+
+ q = t;
+ while((*q++ = *p++));
+
+ return 1;
+}
+
+
+
+/* @funcstatic GPS_Input_Read_Line ************************************
+**
+** Read line from a file ignoring comments and newlines
+** Remove trailing newline
+**
+** @param [w] s [char *] string
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] success
+************************************************************************/
+
+static int32 GPS_Input_Read_Line(char *s, FILE *inf)
+{
+ int32 len;
+
+ while(fgets(s,GPS_ARB_LEN,inf))
+ {
+ if(*s=='#' || *s=='\n')
+ continue;
+ len = strlen(s);
+ if(s[len-1]=='\n')
+ {
+ s[len-1]='\0';
+ return len-1;
+ }
+ return len;
+ }
+
+ return 0;
+}
+
+
+
+/* @func GPS_Input_Get_Almanac ****************************************
+**
+** Construct an almanac array from a file
+**
+** @param [w] alm [GPS_PAlmanac **] pointer to almanac array
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+
+int32 GPS_Input_Get_Almanac(GPS_PAlmanac **alm, FILE *inf)
+{
+ char s[GPS_ARB_LEN];
+ int32 n;
+ int32 type;
+ int32 i;
+ int32 d;
+ float f;
+ char *p;
+
+ gps_errno = INPUT_ERROR;
+
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+
+ if(sscanf(s,"Almanac %d%d",(int *)&n,(int *)&type)!=2)
+ return gps_errno;
+
+ if(!type)
+ {
+ if(!(*alm = (GPS_PAlmanac *) malloc(32*sizeof(GPS_PAlmanac *))))
+ return MEMORY_ERROR;
+ for(i=0;i<32;++i)
+ {
+ if(!((*alm)[i] = GPS_Almanac_New()))
+ return MEMORY_ERROR;
+
+ (*alm)[i]->svid = i;
+ (*alm)[i]->wn = -1;
+ }
+ }
+ else
+ {
+ if(!(*alm = (GPS_PAlmanac *) malloc(n*sizeof(GPS_PAlmanac *))))
+ return MEMORY_ERROR;
+ for(i=0;i<32;++i)
+ if(!((*alm)[i] = GPS_Almanac_New()))
+ return MEMORY_ERROR;
+ }
+
+ for(i=0;i<n;++i)
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ {
+ if(!type)
+ break;
+ else
+ return gps_error;
+ }
+
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ --d;
+
+ if(!type)
+ while((*alm)[i]->svid!=d) ++i;
+ (*alm)[i]->svid=d;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*alm)[i]->wn = d;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%f",&f)!=1)
+ return gps_errno;
+ (*alm)[i]->toa = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%f",&f)!=1)
+ return gps_errno;
+ (*alm)[i]->af0 = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%f",&f)!=1)
+ return gps_errno;
+ (*alm)[i]->af1 = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%f",&f)!=1)
+ return gps_errno;
+ (*alm)[i]->e = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%f",&f)!=1)
+ return gps_errno;
+ (*alm)[i]->sqrta = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%f",&f)!=1)
+ return gps_errno;
+ (*alm)[i]->m0 = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%f",&f)!=1)
+ return gps_errno;
+ (*alm)[i]->w = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%f",&f)!=1)
+ return gps_errno;
+ (*alm)[i]->omg0 = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%f",&f)!=1)
+ return gps_errno;
+ (*alm)[i]->odot = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%f",&f)!=1)
+ return gps_errno;
+ (*alm)[i]->i = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*alm)[i]->hlth=d;
+ }
+
+ if(!type)
+ n = 32;
+
+ return n;
+}
+
+
+
+/* @func GPS_Input_Get_Waypoint *****************************************
+**
+** Construct a waypoint array from a file
+**
+** @param [w] way [GPS_PWay **] pointer to waypoint array
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+
+int32 GPS_Input_Get_Waypoint(GPS_PWay **way, FILE *inf)
+{
+ char s[GPS_ARB_LEN];
+ int32 n;
+ int32 type;
+ int32 i;
+ long pos;
+ int32 ret;
+
+ gps_errno = INPUT_ERROR;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(sscanf(s,"Waypoints Type: %d",(int *)&type)!=1)
+ return gps_errno;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(strncmp(s,"Start",5))
+ return gps_errno;
+
+ pos = ftell(inf);
+ n = 0;
+ while(strncmp(s,"End",3))
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(strstr(s,"Latitude"))
+ ++n;
+ }
+ fseek(inf,pos,0);
+
+ if(!(*way=(GPS_PWay *)malloc(n*sizeof(GPS_PWay *))))
+ return MEMORY_ERROR;
+ for(i=0;i<n;++i)
+ {
+ if(!((*way)[i]=GPS_Way_New()))
+ return MEMORY_ERROR;
+ (*way)[i]->prot = type;
+ }
+
+
+ for(i=0;i<n;++i)
+ {
+ switch(type)
+ {
+ case 100:
+ ret = GPS_Input_Get_D100(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 101:
+ ret = GPS_Input_Get_D101(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 102:
+ ret = GPS_Input_Get_D102(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 103:
+ ret = GPS_Input_Get_D103(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 104:
+ ret = GPS_Input_Get_D104(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 105:
+ ret = GPS_Input_Get_D105(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 106:
+ ret = GPS_Input_Get_D106(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 107:
+ ret = GPS_Input_Get_D107(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 108:
+ ret = GPS_Input_Get_D108(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 150:
+ ret = GPS_Input_Get_D150(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 151:
+ ret = GPS_Input_Get_D151(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 152:
+ ret = GPS_Input_Get_D152(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 154:
+ ret = GPS_Input_Get_D154(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 155:
+ ret = GPS_Input_Get_D155(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ default:
+ GPS_Error("Input_Get_Waypoints: Unknown protocol");
+ return PROTOCOL_ERROR;
+ }
+ }
+
+ return n;
+}
+
+
+
+/* @func GPS_Input_Get_Proximity *************************************
+**
+** Construct a proximity waypoint array from a file
+**
+** @param [w] way [GPS_PWay **] pointer to proximity waypoint array
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+
+int32 GPS_Input_Get_Proximity(GPS_PWay **way, FILE *inf)
+{
+ char s[GPS_ARB_LEN];
+ int32 n;
+ int32 type;
+ int32 i;
+ long pos;
+ int32 ret;
+ double f;
+ char *p;
+
+ gps_errno = INPUT_ERROR;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(sscanf(s,"Waypoints Type: %d",(int *)&type)!=1)
+ return gps_errno;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(strncmp(s,"Start",5))
+ return gps_errno;
+
+ pos = ftell(inf);
+ n = 0;
+ while(strncmp(s,"End",3))
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(strstr(s,"Latitude"))
+ ++n;
+ }
+ fseek(inf,pos,0);
+
+ if(!(*way=(GPS_PWay *)malloc(n*sizeof(GPS_PWay *))))
+ return MEMORY_ERROR;
+ for(i=0;i<n;++i)
+ {
+ if(!((*way)[i]=GPS_Way_New()))
+ return MEMORY_ERROR;
+ (*way)[i]->prot = type;
+ }
+
+ for(i=0;i<n;++i)
+ {
+ switch(type)
+ {
+ case 400:
+ ret = GPS_Input_Get_D100(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 101:
+ ret = GPS_Input_Get_D101(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 102:
+ ret = GPS_Input_Get_D102(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 403:
+ ret = GPS_Input_Get_D103(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 104:
+ ret = GPS_Input_Get_D104(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 105:
+ ret = GPS_Input_Get_D105(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 106:
+ ret = GPS_Input_Get_D106(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 107:
+ ret = GPS_Input_Get_D107(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 108:
+ ret = GPS_Input_Get_D108(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 450:
+ ret = GPS_Input_Get_D150(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 151:
+ ret = GPS_Input_Get_D151(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 152:
+ ret = GPS_Input_Get_D152(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 154:
+ ret = GPS_Input_Get_D154(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 155:
+ ret = GPS_Input_Get_D155(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ default:
+ GPS_Error("Input_Get_Waypoints: Unknown protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)[i]->dst = f;
+ }
+
+ return n;
+}
+
+
+
+/* @funcstatic GPS_Input_Get_D100 ************************************
+**
+** Get a D100 Entry
+**
+** @param [w] way [GPS_PWay *] pointer to waypoint entry
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+static int32 GPS_Input_Get_D100(GPS_PWay *way, FILE *inf)
+{
+ char s[GPS_ARB_LEN];
+ char *p;
+
+ double f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->ident,6,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lat = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lon = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->cmnt,40,s);
+
+ return 1;
+}
+
+
+
+/* @funcstatic GPS_Input_Get_D101 ************************************
+**
+** Get a D101 Entry
+**
+** @param [w] way [GPS_PWay *] pointer to waypoint entry
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+static int32 GPS_Input_Get_D101(GPS_PWay *way, FILE *inf)
+{
+ char s[GPS_ARB_LEN];
+ char *p;
+
+ double f;
+ int32 d;
+
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->ident,6,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lat = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lon = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->cmnt,40,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->smbl = d;
+
+ return 1;
+}
+
+
+
+/* @funcstatic GPS_Input_Get_D102 ************************************
+**
+** Get a D102 Entry
+**
+** @param [w] way [GPS_PWay *] pointer to waypoint entry
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+static int32 GPS_Input_Get_D102(GPS_PWay *way, FILE *inf)
+{
+ return GPS_Input_Get_D101(way,inf);
+}
+
+
+
+/* @funcstatic GPS_Input_Get_D103 ************************************
+**
+** Get a D103 Entry
+**
+** @param [w] way [GPS_PWay *] pointer to waypoint entry
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+static int32 GPS_Input_Get_D103(GPS_PWay *way, FILE *inf)
+{
+ char s[GPS_ARB_LEN];
+ char *p;
+
+ double f;
+ int32 d;
+
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->ident,6,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lat = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lon = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->cmnt,40,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->smbl = d;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->dspl = d;
+
+ return 1;
+}
+
+
+
+/* @funcstatic GPS_Input_Get_D104 ************************************
+**
+** Get a D104 Entry
+**
+** @param [w] way [GPS_PWay *] pointer to waypoint entry
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+static int32 GPS_Input_Get_D104(GPS_PWay *way, FILE *inf)
+{
+ return GPS_Input_Get_D103(way,inf);
+}
+
+
+
+/* @funcstatic GPS_Input_Get_D105 ************************************
+**
+** Get a D105 Entry
+**
+** @param [w] way [GPS_PWay *] pointer to waypoint entry
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+static int32 GPS_Input_Get_D105(GPS_PWay *way, FILE *inf)
+{
+ char s[GPS_ARB_LEN];
+ char *p;
+
+ double f;
+ int32 d;
+
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lat = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lon = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->smbl = d;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_Strnull((*way)->wpt_ident,s);
+
+ return 1;
+}
+
+
+
+/* @funcstatic GPS_Input_Get_D106 ************************************
+**
+** Get a D106 Entry
+**
+** @param [w] way [GPS_PWay *] pointer to waypoint entry
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+static int32 GPS_Input_Get_D106(GPS_PWay *way, FILE *inf)
+{
+ char s[GPS_ARB_LEN];
+ char *p;
+
+ double f;
+ int32 d;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->wpt_class = d;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((char *)(*way)->subclass,13,s);
+
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->ident,6,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lat = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lon = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->smbl = d;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_Strnull((*way)->wpt_ident,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_Strnull((*way)->lnk_ident,s);
+
+ return 1;
+}
+
+
+
+/* @funcstatic GPS_Input_Get_D107 ************************************
+**
+** Get a D107 Entry
+**
+** @param [w] way [GPS_PWay *] pointer to waypoint entry
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+static int32 GPS_Input_Get_D107(GPS_PWay *way, FILE *inf)
+{
+ char s[GPS_ARB_LEN];
+ char *p;
+
+ int32 d;
+ int32 ret;
+
+ if((ret=GPS_Input_Get_D103(way,inf))<0)
+ return ret;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->colour = d;
+
+ return 1;
+}
+
+
+
+/* @funcstatic GPS_Input_Get_D108 ************************************
+**
+** Get a D108 Entry
+**
+** @param [w] way [GPS_PWay *] pointer to waypoint entry
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+static int32 GPS_Input_Get_D108(GPS_PWay *way, FILE *inf)
+{
+ char s[GPS_ARB_LEN];
+ char *p;
+ double f;
+ int32 d;
+ int32 xc;
+
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_Strnull((*way)->ident,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lat = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lon = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->colour = d;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->dspl = d;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->smbl = d;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->alt = d;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->dpth = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->state,2,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->cc,2,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->wpt_class = d;
+ xc = d;
+
+ if(xc>=0x80 && xc<=0x85)
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((char *)(*way)->subclass,18,s);
+ }
+ else
+ {
+ GPS_Util_Put_Short((*way)->subclass,0);
+ GPS_Util_Put_Int((*way)->subclass+2,0);
+ GPS_Util_Put_Uint((*way)->subclass+6,0xffffffff);
+ GPS_Util_Put_Uint((*way)->subclass+10,0xffffffff);
+ GPS_Util_Put_Uint((*way)->subclass+14,0xffffffff);
+ }
+
+ if(!xc)
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_Strnull((*way)->cmnt,s);
+ }
+
+ if(xc>=0x40 && xc<=0x46)
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_Strnull((*way)->facility,s);
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_Strnull((*way)->city,s);
+ }
+
+ if(xc==0x83)
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_Strnull((*way)->addr,s);
+ }
+
+ if(xc==0x82)
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_Strnull((*way)->cross_road,s);
+ }
+
+ return 1;
+}
+
+
+
+/* @funcstatic GPS_Input_Get_D150 ************************************
+**
+** Get a D150 Entry
+**
+** @param [w] way [GPS_PWay *] pointer to waypoint entry
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+static int32 GPS_Input_Get_D150(GPS_PWay *way, FILE *inf)
+{
+ char s[GPS_ARB_LEN];
+ char *p;
+
+ double f;
+ int32 d;
+ int32 cl=0;
+
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->ident,6,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lat = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lon = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->cmnt,40,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->wpt_class = cl = d;
+
+ if(cl != 4)
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->cc,2,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->city,24,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->state,2,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->name,30,s);
+ }
+
+ if(!cl)
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->alt = d;
+ }
+
+ return 1;
+}
+
+
+
+/* @funcstatic GPS_Input_Get_D151 ************************************
+**
+** Get a D151 Entry
+**
+** @param [w] way [GPS_PWay *] pointer to waypoint entry
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+static int32 GPS_Input_Get_D151(GPS_PWay *way, FILE *inf)
+{
+ char s[GPS_ARB_LEN];
+ char *p;
+
+ double f;
+ int32 d;
+ int32 cl=0;
+
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->ident,6,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lat = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lon = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->cmnt,40,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->wpt_class = cl = d;
+
+ if(cl != 2)
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->cc,2,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->city,24,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->state,2,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->name,30,s);
+ }
+
+ if(!cl)
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->alt = d;
+ }
+
+ return 1;
+}
+
+
+/* @funcstatic GPS_Input_Get_D152 ************************************
+**
+** Get a D152 Entry
+**
+** @param [w] way [GPS_PWay *] pointer to waypoint entry
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+static int32 GPS_Input_Get_D152(GPS_PWay *way, FILE *inf)
+{
+ return GPS_Input_Get_D150(way,inf);
+}
+
+
+
+/* @funcstatic GPS_Input_Get_D154 ************************************
+**
+** Get a D154 Entry
+**
+** @param [w] way [GPS_PWay *] pointer to waypoint entry
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+static int32 GPS_Input_Get_D154(GPS_PWay *way, FILE *inf)
+{
+ char s[GPS_ARB_LEN];
+ char *p;
+
+ double f;
+ int32 d;
+ int32 cl=0;
+
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->ident,6,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lat = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lon = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->cmnt,40,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->smbl = d;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->wpt_class = cl = d;
+
+ if(cl != 4 && cl != 8)
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->cc,2,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->city,24,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->state,2,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->name,30,s);
+ }
+
+ if(!cl)
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->alt = d;
+ }
+
+ return 1;
+}
+
+
+
+/* @funcstatic GPS_Input_Get_D155 ************************************
+**
+** Get a D155 Entry
+**
+** @param [w] way [GPS_PWay *] pointer to waypoint entry
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+static int32 GPS_Input_Get_D155(GPS_PWay *way, FILE *inf)
+{
+ char s[GPS_ARB_LEN];
+ char *p;
+
+ double f;
+ int32 d;
+ int32 cl=0;
+
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->ident,6,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lat = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*way)->lon = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->cmnt,40,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->smbl = d;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->dspl = d;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->wpt_class = cl = d;
+
+ if(cl != 4 && cl != 8)
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->cc,2,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->city,24,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->state,2,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)->name,30,s);
+ }
+
+ if(!cl)
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_errno;
+ (*way)->alt = d;
+ }
+
+ return 1;
+}
+
+
+
+/* @func GPS_Input_Get_Track *******************************************
+**
+** Construct a travk array from a file
+**
+** @param [w] trk [GPS_PTrack **] pointer to track array
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+
+int32 GPS_Input_Get_Track(GPS_PTrack **trk, FILE *inf)
+{
+ char s[GPS_ARB_LEN];
+ int32 n;
+ int32 i;
+ long pos;
+ int32 a;
+ int32 d;
+
+ gps_errno = INPUT_ERROR;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(strncmp(s,"Track log",9))
+ return gps_errno;
+
+ if(sscanf(s,"Track log %d%d",(int *)&a,(int *)&d)!=2)
+ return INPUT_ERROR;
+
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(strncmp(s,"Start",5))
+ return gps_errno;
+
+ pos = ftell(inf);
+ n = 0;
+ while(strncmp(s,"End",3))
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(strstr(s,"Latitude"))
+ ++n;
+ if(strstr(s,"Header"))
+ ++n;
+ }
+ fseek(inf,pos,0);
+
+
+ if(!(*trk=(GPS_PTrack *)malloc(n*sizeof(GPS_PTrack *))))
+ return MEMORY_ERROR;
+ for(i=0;i<n;++i)
+ if(!((*trk)[i]=GPS_Track_New()))
+ return MEMORY_ERROR;
+
+
+ switch(a)
+ {
+ case pA300:
+ break;
+ case pA301:
+ return GPS_Input_Get_Track301(trk, inf, d, n);
+ default:
+ return INPUT_ERROR;
+ }
+
+
+
+ for(i=0;i<n;++i)
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+
+ if(!strcmp(s,"New track"))
+ {
+ (*trk)[i]->tnew=1;
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ }
+ else
+ (*trk)[i]->tnew=0;
+
+ switch(d)
+ {
+ case pD300:
+ GPS_Input_Get_D300(&((*trk)[i]),inf,s);
+ break;
+ case pD301:
+ GPS_Input_Get_D301(&((*trk)[i]),inf,s);
+ break;
+ default:
+ return PROTOCOL_ERROR;
+ }
+ }
+
+ return n;
+}
+
+
+
+/* @funcstatic GPS_Input_Get_Track301 **********************************
+**
+** Construct a travk array from a file
+**
+** @param [w] trk [GPS_PTrack **] pointer to track array
+** @param [r] inf [FILE *] stream
+** @param [r] type [int32] data type
+** @param [r] n [int32] number of tracks
+**
+** @return [int32] number of entries
+************************************************************************/
+
+static int32 GPS_Input_Get_Track301(GPS_PTrack **trk, FILE *inf, int32 type,
+ int32 n)
+{
+ char s[GPS_ARB_LEN];
+ int32 i;
+ char *p;
+ int32 x;
+
+ for(i=0;i<n;++i)
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(!strcmp(s,"Header"))
+ {
+ (*trk)[i]->ishdr = 1;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_Strnull((*trk)[i]->trk_ident,s);
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&x)!=1)
+ return INPUT_ERROR;
+ (*trk)[i]->dspl = x;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&x)!=1)
+ return INPUT_ERROR;
+ (*trk)[i]->colour = x;
+
+ continue;
+ }
+
+ (*trk)[i]->ishdr = 0;
+
+ if(!strcmp(s,"New track"))
+ {
+ (*trk)[i]->tnew=1;
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ }
+ else
+ (*trk)[i]->tnew=0;
+
+ switch(type)
+ {
+ case pD300:
+ GPS_Input_Get_D300(&((*trk)[i]),inf,s);
+ break;
+ case pD301:
+ GPS_Input_Get_D301(&((*trk)[i]),inf,s);
+ break;
+ default:
+ return PROTOCOL_ERROR;
+ }
+ }
+
+ return n;
+}
+
+
+
+/* @funcstatic GPS_Input_Get_D300 **********************************
+**
+** Construct a track from a file
+**
+** @param [w] trk [GPS_PTrack *] pointer to track
+** @param [r] inf [FILE *] stream
+** @param [w] s [char *] input line
+**
+** @return [int32] number of entries
+************************************************************************/
+
+static int32 GPS_Input_Get_D300(GPS_PTrack *trk, FILE *inf, char *s)
+{
+ char *p;
+ double f;
+
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*trk)->lat = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*trk)->lon = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ (*trk)->Time = 0;
+
+ return 1;
+}
+
+
+
+/* @funcstatic GPS_Input_Get_D301 **********************************
+**
+** Construct a track from a file
+**
+** @param [w] trk [GPS_PTrack *] pointer to track
+** @param [r] inf [FILE *] stream
+** @param [w] s [char *] input line
+**
+** @return [int32] number of entries
+************************************************************************/
+
+static int32 GPS_Input_Get_D301(GPS_PTrack *trk, FILE *inf, char *s)
+{
+ char *p;
+ double f;
+
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*trk)->lat = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*trk)->lon = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ (*trk)->Time = 0;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*trk)->alt = f;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ p=strchr(s,':');
+ if(sscanf(p+1,"%lf",&f)!=1)
+ return gps_errno;
+ (*trk)->dpth = f;
+
+ return 1;
+}
+
+
+
+/* @func GPS_Input_Get_Route *******************************************
+**
+** Construct a route array from a file
+**
+** @param [w] way [GPS_PWay **] pointer to waypoint array
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+
+int32 GPS_Input_Get_Route(GPS_PWay **way, FILE *inf)
+{
+ char s[GPS_ARB_LEN];
+ int32 n;
+ int32 type;
+ int32 rtype=0;
+ int32 atype=0;
+ int32 i;
+ long pos;
+ int32 ret;
+ char *p;
+ int32 d;
+
+ gps_errno = INPUT_ERROR;
+
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(sscanf(s,"Route log %d",(int *)&atype)!=1)
+ return gps_errno;
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+
+ switch(atype)
+ {
+ case pA200:
+ break;
+ case pA201:
+ return GPS_Input_Get_Route201(way,inf);
+ default:
+ GPS_Error("GPS_Input_Get_Route: Unknown protocol");
+ return PROTOCOL_ERROR;
+ }
+
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(sscanf(s,"Route Type: %d",(int *)&rtype)!=1)
+ return gps_errno;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(sscanf(s,"Waypoints Type: %d",(int *)&type)!=1)
+ return gps_errno;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(strncmp(s,"Start",5))
+ return gps_errno;
+
+ pos = ftell(inf);
+ n = 1;
+ while(strncmp(s,"End",3))
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(strstr(s,"Latitude") || strstr(s,"Route"))
+ ++n;
+ }
+ fseek(inf,0L,0);
+
+
+ if(!(*way=(GPS_PWay *)malloc(n*sizeof(GPS_PWay *))))
+ return MEMORY_ERROR;
+ for(i=0;i<n;++i)
+ {
+ if(!((*way)[i]=GPS_Way_New()))
+ return MEMORY_ERROR;
+ (*way)[i]->prot = type;
+ }
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+
+ for(i=0;i<n;++i)
+ {
+ (*way)[i]->rte_prot = rtype;
+ (*way)[i]->islink = 0;
+ if(strstr(s,"Route"))
+ {
+ (*way)[i]->isrte = 1;
+ switch(rtype)
+ {
+ case 200:
+ p = strchr(s,':');
+ p = strchr(p+1,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_error;
+ (*way)[i]->rte_num=d;
+ break;
+ case 201:
+ p = strchr(s,':');
+ p = strchr(p+1,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_error;
+ (*way)[i]->rte_num=d;
+ ++p;
+ GPS_Input_Load_String((*way)[i]->rte_cmnt,20,p+2);
+ break;
+ case 202:
+ p = strchr(s,':');
+ p = strchr(p+1,':');
+ GPS_Input_Load_Strnull((*way)[i]->rte_ident,p+1);
+ break;
+ }
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ continue;
+ }
+ else
+ (*way)[i]->isrte=0;
+
+ if(strstr(s,"End"))
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+
+ continue;
+ }
+
+
+ switch(type)
+ {
+ case 100:
+ ret = GPS_Input_Get_D100(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 101:
+ ret = GPS_Input_Get_D101(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 102:
+ ret = GPS_Input_Get_D102(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 103:
+ ret = GPS_Input_Get_D103(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 104:
+ ret = GPS_Input_Get_D104(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 105:
+ ret = GPS_Input_Get_D105(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 106:
+ ret = GPS_Input_Get_D106(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 107:
+ ret = GPS_Input_Get_D107(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 108:
+ ret = GPS_Input_Get_D108(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 150:
+ ret = GPS_Input_Get_D150(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 151:
+ ret = GPS_Input_Get_D151(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 152:
+ ret = GPS_Input_Get_D152(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 154:
+ ret = GPS_Input_Get_D154(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 155:
+ ret = GPS_Input_Get_D155(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ default:
+ GPS_Error("Input_Get_Waypoints: Unknown protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ }
+
+ return n;
+}
+
+
+
+/* @funcstatic GPS_Input_Get_Route201 ***********************************
+**
+** Construct a route array from a file
+**
+** @param [w] way [GPS_PWay **] pointer to waypoint array
+** @param [r] inf [FILE *] stream
+**
+** @return [int32] number of entries
+************************************************************************/
+
+static int32 GPS_Input_Get_Route201(GPS_PWay **way, FILE *inf)
+{
+ char s[GPS_ARB_LEN];
+ int32 n;
+ int32 type;
+ int32 rtype;
+ int32 i;
+ long pos;
+ int32 ret;
+ char *p;
+ int32 d;
+
+ gps_errno = INPUT_ERROR;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(sscanf(s,"Route Type: %d",(int *)&rtype)!=1)
+ return gps_errno;
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(sscanf(s,"Waypoints Type: %d",(int *)&type)!=1)
+ return gps_errno;
+
+
+ pos = ftell(inf);
+ n = 1;
+ while(strncmp(s,"End",3))
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(strstr(s,"Latitude") || strstr(s,"Route") || strstr(s,"Link Class"))
+ ++n;
+ }
+ fseek(inf,0L,0);
+
+ if(!(*way=(GPS_PWay *)malloc(n*sizeof(GPS_PWay *))))
+ return MEMORY_ERROR;
+ for(i=0;i<n;++i)
+ {
+ if(!((*way)[i]=GPS_Way_New()))
+ return MEMORY_ERROR;
+ (*way)[i]->prot = type;
+ }
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+
+ for(i=0;i<n;++i)
+ {
+ (*way)[i]->rte_prot = rtype;
+ (*way)[i]->islink = 0;
+ if(strstr(s,"Route"))
+ {
+ (*way)[i]->isrte = 1;
+ switch(rtype)
+ {
+ case 200:
+ p = strchr(s,':');
+ p = strchr(p+1,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_error;
+ (*way)[i]->rte_num=d;
+ break;
+ case 201:
+ p = strchr(s,':');
+ p = strchr(p+1,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_error;
+ (*way)[i]->rte_num=d;
+ p = strchr(p+1,':');
+ GPS_Input_Load_String((*way)[i]->rte_cmnt,20,p+1);
+ break;
+ case 202:
+ p = strchr(s,':');
+ p = strchr(p+1,':');
+ GPS_Input_Load_Strnull((*way)[i]->rte_ident,p+1);
+ break;
+ }
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ continue;
+ }
+ else
+ (*way)[i]->isrte=0;
+
+
+
+ if(strstr(s,"Link Class"))
+ {
+ (*way)[i]->islink = 1;
+
+ p = strchr(s,':');
+ if(sscanf(p+1,"%d",(int *)&d)!=1)
+ return gps_error;
+ (*way)[i]->rte_link_class=d;
+
+ if(!((*way)[i]->rte_link_class==3 || (*way)[i]->rte_link_class
+ ==0xff))
+ {
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_String((*way)[i]->rte_link_subclass,18,s);
+ }
+ else
+ {
+ GPS_Util_Put_Short((UC *)(*way)[i]->rte_link_subclass,0);
+ GPS_Util_Put_Int((UC *)(*way)[i]->rte_link_subclass+2,0);
+ GPS_Util_Put_Uint((UC *)(*way)[i]->rte_link_subclass+6,
+ 0xffffffff);
+ GPS_Util_Put_Uint((UC *)(*way)[i]->rte_link_subclass+10,
+ 0xffffffff);
+ GPS_Util_Put_Uint((UC *)(*way)[i]->rte_link_subclass+14,
+ 0xffffffff);
+ }
+
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ GPS_Input_Load_Strnull((*way)[i]->rte_link_ident,s);
+
+ continue;
+ }
+ else
+ (*way)[i]->islink=0;
+
+
+ if(strstr(s,"End"))
+ {
+ GPS_Error("Get_Route201: Unexpected End");
+ return INPUT_ERROR;
+ }
+
+
+ switch(type)
+ {
+ case 100:
+ ret = GPS_Input_Get_D100(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 101:
+ ret = GPS_Input_Get_D101(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 102:
+ ret = GPS_Input_Get_D102(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 103:
+ ret = GPS_Input_Get_D103(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 104:
+ ret = GPS_Input_Get_D104(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 105:
+ ret = GPS_Input_Get_D105(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 106:
+ ret = GPS_Input_Get_D106(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 107:
+ ret = GPS_Input_Get_D107(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 108:
+ ret = GPS_Input_Get_D108(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 150:
+ ret = GPS_Input_Get_D150(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 151:
+ ret = GPS_Input_Get_D151(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 152:
+ ret = GPS_Input_Get_D152(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 154:
+ ret = GPS_Input_Get_D154(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ case 155:
+ ret = GPS_Input_Get_D155(&((*way)[i]),inf);
+ if(ret<0) return gps_errno;
+ break;
+ default:
+ GPS_Error("Input_Get_Waypoints: Unknown protocol");
+ return PROTOCOL_ERROR;
+ }
+ if(!GPS_Input_Read_Line(s,inf))
+ return gps_errno;
+ }
+
+ return n;
+}
--- /dev/null
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef gpsinput_h
+#define gpsinput_h
+
+
+#include "gps.h"
+
+int32 GPS_Input_Get_Almanac(GPS_PAlmanac **alm, FILE *inf);
+int32 GPS_Input_Get_Waypoint(GPS_PWay **way, FILE *inf);
+int32 GPS_Input_Get_Proximity(GPS_PWay **way, FILE *inf);
+int32 GPS_Input_Get_Track(GPS_PTrack **trk, FILE *inf);
+int32 GPS_Input_Get_Route(GPS_PWay **way, FILE *inf);
+
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null
+/********************************************************************
+** @source JEEPS arithmetic/conversion functions
+**
+** @author Copyright (C) 1999 Alan Bleasby
+** @version 1.0
+** @modified Dec 28 1999 Alan Bleasby. First version
+** @@
+**
+** This library is free software; you can redistribute it and/or
+** modify it under the terms of the GNU Library General Public
+** License as published by the Free Software Foundation; either
+** version 2 of the License, or (at your option) any later version.
+**
+** This library is distributed in the hope that it will be useful,
+** but WITHOUT ANY WARRANTY; without even the implied warranty of
+** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+** Library General Public License for more details.
+**
+** You should have received a copy of the GNU Library General Public
+** License along with this library; if not, write to the
+** Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+** Boston, MA 02111-1307, USA.
+********************************************************************/
+#include "gps.h"
+#include <math.h>
+#include <string.h>
+#include "gpsdatum.h"
+
+
+
+static int32 GPS_Math_LatLon_To_UTM_Param(double lat, double lon, int32 *zone,
+ char *zc, double *Mc, double *E0,
+ double *N0, double *F0);
+static int32 GPS_Math_UTM_Param_To_Mc(int32 zone, char zc, double *Mc,
+ double *E0, double *N0, double *F0);
+
+
+
+/* @func GPS_Math_Deg_To_Rad *******************************************
+**
+** Convert degrees to radians
+**
+** @param [r] v [double] degrees
+**
+** @return [double] radians
+************************************************************************/
+
+double GPS_Math_Deg_To_Rad(double v)
+{
+ return v*(double)((double)GPS_PI/(double)180.);
+}
+
+
+
+/* @func GPS_Math_Rad_To_Deg *******************************************
+**
+** Convert radians to degrees
+**
+** @param [r] v [double] radians
+**
+** @return [double] degrees
+************************************************************************/
+
+double GPS_Math_Rad_To_Deg(double v)
+{
+ return v*(double)((double)180./(double)GPS_PI);
+}
+
+
+
+/* @func GPS_Math_Deg_To_DegMin *****************************************
+**
+** Convert degrees to degrees and minutes
+**
+** @param [r] v [double] degrees
+** @param [w] d [int32 *] whole degrees
+** @param [w] m [double *] minutes
+**
+** @return [void]
+************************************************************************/
+
+void GPS_Math_Deg_To_DegMin(double v, int32 *d, double *m)
+{
+ int32 sign;
+
+ if(v<(double)0.)
+ {
+ v *= (double)-1.;
+ sign = 1;
+ }
+ else
+ sign = 0;
+
+ *d = (int32)v;
+ *m = (v-(double)*d) * (double)60.0;
+ if(*m>(double)59.999)
+ {
+ ++*d;
+ *m = (double)0.0;
+ }
+
+ if(sign)
+ *d = -*d;
+
+ return;
+}
+
+
+
+/* @func GPS_Math_DegMin_To_Deg *****************************************
+**
+** Convert degrees and minutes to degrees
+**
+** @param [r] d [int32] whole degrees
+** @param [r] m [double] minutes
+** @param [w] deg [double *] degrees
+**
+** @return [void]
+************************************************************************/
+
+void GPS_Math_DegMin_To_Deg(int32 d, double m, double *deg)
+{
+
+ *deg = ((double)abs(d)) + m / (double)60.0;
+ if(d<0)
+ *deg = -*deg;
+
+ return;
+}
+
+
+
+/* @func GPS_Math_Deg_To_DegMinSec *************************************
+**
+** Convert degrees to degrees, minutes and seconds
+**
+** @param [r] v [double] degrees
+** @param [w] d [int32 *] whole degrees
+** @param [w] m [int32 *] whole minutes
+** @param [w] s [double *] seconds
+**
+** @return [void]
+************************************************************************/
+
+void GPS_Math_Deg_To_DegMinSec(double v, int32 *d, int32 *m, double *s)
+{
+ int32 sign;
+ double t;
+
+ if(v<(double)0.)
+ {
+ v *= (double)-1.;
+ sign = 1;
+ }
+ else
+ sign = 0;
+
+ *d = (int32)v;
+ t = (v -(double)*d) * (double)60.0;
+ *s = (t-(double)*m) * (double)60.0;
+
+ if(*s>(double)59.999)
+ {
+ ++t;
+ *s = (double)0.0;
+ }
+
+
+ if(t>(double)59.999)
+ {
+ ++*d;
+ t = 0;
+ }
+
+ *m = (int32)t;
+
+ if(sign)
+ *d = -*d;
+
+ return;
+}
+
+
+
+/* @func GPS_Math_DegMinSec_To_Deg *****************************************
+**
+** Convert degrees, minutes and seconds to degrees
+**
+** @param [r] d [int32] whole degrees
+** @param [r] m [int32] whole minutes
+** @param [r] s [double] seconds
+** @param [w] deg [double *] degrees
+**
+** @return [void]
+************************************************************************/
+
+void GPS_Math_DegMinSec_To_Deg(int32 d, int32 m, double s, double *deg)
+{
+
+ *deg = ((double)abs(d)) + ((double)m + s / (double)60.0) / (double)60.0;
+ if(d<0)
+ *deg = -*deg;
+
+ return;
+}
+
+
+
+/* @func GPS_Math_Metres_To_Feet *******************************************
+**
+** Convert metres to feet
+**
+** @param [r] v [double] metres
+**
+** @return [double] feet
+************************************************************************/
+
+double GPS_Math_Metres_To_Feet(double v)
+{
+ return v*(double)2.7432;
+}
+
+
+
+/* @func GPS_Math_Feet_To_Metres *******************************************
+**
+** Convert feet to metres
+**
+** @param [r] v [double] feet
+**
+** @return [double] metres
+************************************************************************/
+
+double GPS_Math_Feet_To_Metres(double v)
+{
+ return v/(double)2.7432;
+}
+
+
+
+/* @func GPS_Math_Deg_To_Semi *******************************************
+**
+** Convert degrees to semi-circles
+**
+** @param [r] v [double] degrees
+**
+** @return [int32] semicircles
+************************************************************************/
+
+int32 GPS_Math_Deg_To_Semi(double v)
+{
+ return (int32) (((double)2.147483e9/(double)180)*(double)v);
+}
+
+
+
+/* @func GPS_Math_Semi_To_Deg *******************************************
+**
+** Convert semi-circles to degrees
+**
+** @param [r] v [int32] semi-circles
+**
+** @return [double] degrees
+************************************************************************/
+
+double GPS_Math_Semi_To_Deg(int32 v)
+{
+ return (double) (((double)v/(double)2.147483e9) * (double)180);
+}
+
+
+
+/* @func GPS_Math_Utime_To_Gtime *******************************************
+**
+** Convert Unix time (1970) to GPS time (1990)
+**
+** @param [r] v [time_t] Unix time
+**
+** @return [time_t] GPS time
+************************************************************************/
+
+time_t GPS_Math_Utime_To_Gtime(time_t v)
+{
+ return v-631065600;
+}
+
+
+
+/* @func GPS_Math_Gtime_To_Utime *******************************************
+**
+** Convert GPS time (1990) to Unix time (1970)
+**
+** @param [r] v [time_t] GPS time
+**
+** @return [time_t] Unix time
+************************************************************************/
+
+time_t GPS_Math_Gtime_To_Utime(time_t v)
+{
+ return v+631065600;
+}
+
+
+
+
+/* @func GPS_Math_LatLonH_To_XYZ **********************************
+**
+** Convert latitude and longitude and ellipsoidal height to
+** X, Y & Z coordinates
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [r] H [double] ellipsoidal height (metres)
+** @param [w] x [double *] X
+** @param [w] y [double *] Y
+** @param [w] z [double *] Z
+** @param [r] a [double] semi-major axis (metres)
+** @param [r] b [double] semi-minor axis (metres)
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_LatLonH_To_XYZ(double phi, double lambda, double H,
+ double *x, double *y, double *z,
+ double a, double b)
+{
+ double esq;
+ double nu;
+
+ phi = GPS_Math_Deg_To_Rad(phi);
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+
+
+ esq = ((a*a)-(b*b)) / (a*a);
+
+ nu = a / pow(((double)1.0-esq*sin(phi)*sin(phi)),(double)0.5);
+ *x = (nu+H) * cos(phi) * cos(lambda);
+ *y = (nu+H) * cos(phi) * sin(lambda);
+ *z = (((double)1.0-esq)*nu+H) * sin(phi);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_XYX_To_LatLonH ***************************************
+**
+** Convert XYZ coordinates to latitude and longitude and ellipsoidal height
+**
+** @param [w] phi [double] latitude (deg)
+** @param [w] lambda [double] longitude (deg)
+** @param [w] H [double] ellipsoidal height (metres)
+** @param [r] x [double *] X
+** @param [r] y [double *] Y
+** @param [r] z [double *] Z
+** @param [r] a [double] semi-major axis (metres)
+** @param [r] b [double] semi-minor axis (metres)
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_XYZ_To_LatLonH(double *phi, double *lambda, double *H,
+ double x, double y, double z,
+ double a, double b)
+{
+ double esq;
+ double nu=0.0;
+ double phix;
+ double nphi;
+ double rho;
+ int32 t1=0;
+ int32 t2=0;
+
+ if(x<(double)0 && y>=(double)0) t1=1;
+ if(x<(double)0 && y<(double)0) t2=1;
+
+ rho = pow(((x*x)+(y*y)),(double)0.5);
+ esq = ((a*a)-(b*b)) / (a*a);
+ phix = atan(z/(((double)1.0 - esq) * rho));
+ nphi = (double)1e20;
+
+ while(fabs(phix-nphi)>(double)0.00000000001)
+ {
+ nphi = phix;
+ nu = a / pow(((double)1.0-esq*sin(nphi)*sin(nphi)),(double)0.5);
+ phix = atan((z+esq*nu*sin(nphi))/rho);
+ }
+
+ *phi = GPS_Math_Rad_To_Deg(phix);
+ *H = (rho / cos(phix)) - nu;
+ *lambda = GPS_Math_Rad_To_Deg(atan(y/x));
+
+ if(t1) *lambda += (double)180.0;
+ if(t2) *lambda -= (double)180.0;
+
+ return;
+}
+
+
+
+/* @func GPS_Math_Airy1830LatLonH_To_XYZ **********************************
+**
+** Convert Airy 1830 latitude and longitude and ellipsoidal height to
+** X, Y & Z coordinates
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [r] H [double] ellipsoidal height (metres)
+** @param [w] x [double *] X
+** @param [w] y [double *] Y
+** @param [w] z [double *] Z
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Airy1830LatLonH_To_XYZ(double phi, double lambda, double H,
+ double *x, double *y, double *z)
+{
+ double a = 6377563.396;
+ double b = 6356256.910;
+
+ GPS_Math_LatLonH_To_XYZ(phi,lambda,H,x,y,z,a,b);
+
+ return;
+}
+
+
+
+/* @func GPS_Math_WGS84LatLonH_To_XYZ **********************************
+**
+** Convert WGS84 latitude and longitude and ellipsoidal height to
+** X, Y & Z coordinates
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [r] H [double] ellipsoidal height (metres)
+** @param [w] x [double *] X
+** @param [w] y [double *] Y
+** @param [w] z [double *] Z
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_WGS84LatLonH_To_XYZ(double phi, double lambda, double H,
+ double *x, double *y, double *z)
+{
+ double a = 6378137.000;
+ double b = 6356752.3141;
+
+ GPS_Math_LatLonH_To_XYZ(phi,lambda,H,x,y,z,a,b);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_XYZ_To_Airy1830LatLonH **********************************
+**
+** Convert XYZ to Airy 1830 latitude and longitude and ellipsoidal height
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [r] H [double] ellipsoidal height (metres)
+** @param [w] x [double *] X
+** @param [w] y [double *] Y
+** @param [w] z [double *] Z
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_XYZ_To_Airy1830LatLonH(double *phi, double *lambda, double *H,
+ double x, double y, double z)
+{
+ double a = 6377563.396;
+ double b = 6356256.910;
+
+ GPS_Math_XYZ_To_LatLonH(phi,lambda,H,x,y,z,a,b);
+
+ return;
+}
+
+
+
+/* @func GPS_Math_XYZ_To_WGS84LatLonH **********************************
+**
+** Convert XYZ to WGS84 latitude and longitude and ellipsoidal height
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [r] H [double] ellipsoidal height (metres)
+** @param [w] x [double *] X
+** @param [w] y [double *] Y
+** @param [w] z [double *] Z
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_XYZ_To_WGS84LatLonH(double *phi, double *lambda, double *H,
+ double x, double y, double z)
+{
+ double a = 6378137.000;
+ double b = 66356752.3141;
+
+ GPS_Math_XYZ_To_LatLonH(phi,lambda,H,x,y,z,a,b);
+
+ return;
+}
+
+
+
+
+
+
+
+/* @func GPS_Math_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to eastings and northings
+** Standard Gauss-Kruger Transverse Mercator
+**
+** @param [w] E [double *] easting (metres)
+** @param [w] N [double *] northing (metres)
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [r] N0 [double] true northing origin (metres)
+** @param [r] E0 [double] true easting origin (metres)
+** @param [r] phi0 [double] true latitude origin (deg)
+** @param [r] lambda0 [double] true longitude origin (deg)
+** @param [r] F0 [double] scale factor on central meridian
+** @param [r] a [double] semi-major axis (metres)
+** @param [r] b [double] semi-minor axis (metres)
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_LatLon_To_EN(double *E, double *N, double phi,
+ double lambda, double N0, double E0,
+ double phi0, double lambda0,
+ double F0, double a, double b)
+{
+ double esq;
+ double n;
+ double etasq;
+ double nu;
+ double rho;
+ double M;
+ double I;
+ double II;
+ double III;
+ double IIIA;
+ double IV;
+ double V;
+ double VI;
+
+ double tmp;
+ double tmp2;
+ double fdf;
+ double fde;
+
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ lambda0 = GPS_Math_Deg_To_Rad(lambda0);
+ phi = GPS_Math_Deg_To_Rad(phi);
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+
+ esq = ((a*a)-(b*b)) / (a*a);
+ n = (a-b) / (a+b);
+
+ tmp = (double)1.0 - (esq * sin(phi) * sin(phi));
+ nu = a * F0 * pow(tmp,(double)-0.5);
+ rho = a * F0 * ((double)1.0 - esq) * pow(tmp,(double)-1.5);
+ etasq = (nu / rho) - (double)1.0;
+
+ fdf = (double)5.0 / (double)4.0;
+ tmp = (double)1.0 + n + (fdf * n * n) + (fdf * n * n * n);
+ tmp *= (phi - phi0);
+ tmp2 = (double)3.0*n + (double)3.0*n*n + ((double)21./(double)8.)*n*n*n;
+ tmp2 *= (sin(phi-phi0) * cos(phi+phi0));
+ tmp -= tmp2;
+
+ fde = ((double)15.0 / (double)8.0);
+ tmp2 = ((fde*n*n) + (fde*n*n*n)) * sin((double)2.0 * (phi-phi0));
+ tmp2 *= cos((double)2.0 * (phi+phi0));
+ tmp += tmp2;
+
+ tmp2 = ((double)35.0/(double)24.0) * n * n * n;
+ tmp2 *= sin((double)3.0 * (phi-phi0));
+ tmp2 *= cos((double)3.0 * (phi+phi0));
+ tmp -= tmp2;
+
+ M = b * F0 * tmp;
+ I = M + N0;
+ II = (nu / (double)2.0) * sin(phi) * cos(phi);
+ III = (nu / (double)24.0) * sin(phi) * cos(phi) * cos(phi) * cos(phi);
+ III *= ((double)5.0 - (tan(phi) * tan(phi)) + ((double)9.0 * etasq));
+ IIIA = (nu / (double)720.0) * sin(phi) * pow(cos(phi),(double)5.0);
+ IIIA *= ((double)61.0 - ((double)58.0*tan(phi)*tan(phi)) +
+ pow(tan(phi),(double)4.0));
+ IV = nu * cos(phi);
+
+ tmp = pow(cos(phi),(double)3.0);
+ tmp *= ((nu/rho) - tan(phi) * tan(phi));
+ V = (nu/(double)6.0) * tmp;
+
+ tmp = (double)5.0 - ((double)18.0 * tan(phi) * tan(phi));
+ tmp += tan(phi)*tan(phi)*tan(phi)*tan(phi) + ((double)14.0 * etasq);
+ tmp -= ((double)58.0 * tan(phi) * tan(phi) * etasq);
+ tmp2 = cos(phi)*cos(phi)*cos(phi)*cos(phi)*cos(phi) * tmp;
+ VI = (nu / (double)120.0) * tmp2;
+
+ *N = I + II*(lambda-lambda0)*(lambda-lambda0) +
+ III*pow((lambda-lambda0),(double)4.0) +
+ IIIA*pow((lambda-lambda0),(double)6.0);
+
+ *E = E0 + IV*(lambda-lambda0) + V*pow((lambda-lambda0),(double)3.0) +
+ VI * pow((lambda-lambda0),(double)5.0);
+
+ return;
+}
+
+
+
+/* @func GPS_Math_Airy1830MLatLonToINGEN ************************************
+**
+** Convert Modified Airy 1830 datum latitude and longitude to Irish
+** National Grid Eastings and Northings
+**
+** @param [r] phi [double] modified Airy latitude (deg)
+** @param [r] lambda [double] modified Airy longitude (deg)
+** @param [w] E [double *] NG easting (metres)
+** @param [w] N [double *] NG northing (metres)
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Airy1830M_LatLonToINGEN(double phi, double lambda, double *E,
+ double *N)
+{
+ double N0 = 250000;
+ double E0 = 200000;
+ double F0 = 1.000035;
+ double phi0 = 53.5;
+ double lambda0 = -8.;
+ double a = 6377340.189;
+ double b = 6356034.447;
+
+ GPS_Math_LatLon_To_EN(E,N,phi,lambda,N0,E0,phi0,lambda0,F0,a,b);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Airy1830LatLonToNGEN **************************************
+**
+** Convert Airy 1830 datum latitude and longitude to UK Ordnance Survey
+** National Grid Eastings and Northings
+**
+** @param [r] phi [double] WGS84 latitude (deg)
+** @param [r] lambda [double] WGS84 longitude (deg)
+** @param [w] E [double *] NG easting (metres)
+** @param [w] N [double *] NG northing (metres)
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Airy1830LatLonToNGEN(double phi, double lambda, double *E,
+ double *N)
+{
+ double N0 = -100000;
+ double E0 = 400000;
+ double F0 = 0.9996012717;
+ double phi0 = 49.;
+ double lambda0 = -2.;
+ double a = 6377563.396;
+ double b = 6356256.910;
+
+ GPS_Math_LatLon_To_EN(E,N,phi,lambda,N0,E0,phi0,lambda0,F0,a,b);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_EN_To_LatLon **************************************
+**
+** Convert Eastings and Northings to latitude and longitude
+**
+** @param [w] E [double] NG easting (metres)
+** @param [w] N [double] NG northing (metres)
+** @param [r] phi [double *] Airy latitude (deg)
+** @param [r] lambda [double *] Airy longitude (deg)
+** @param [r] N0 [double] true northing origin (metres)
+** @param [r] E0 [double] true easting origin (metres)
+** @param [r] phi0 [double] true latitude origin (deg)
+** @param [r] lambda0 [double] true longitude origin (deg)
+** @param [r] F0 [double] scale factor on central meridian
+** @param [r] a [double] semi-major axis (metres)
+** @param [r] b [double] semi-minor axis (metres)
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double N0, double E0,
+ double phi0, double lambda0,
+ double F0, double a, double b)
+{
+ double esq;
+ double n;
+ double etasq;
+ double nu;
+ double rho;
+ double M;
+ double VII;
+ double VIII;
+ double IX;
+ double X;
+ double XI;
+ double XII;
+ double XIIA;
+ double phix;
+ double nphi=0.0;
+
+ double tmp;
+ double tmp2;
+ double fdf;
+ double fde;
+
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ lambda0 = GPS_Math_Deg_To_Rad(lambda0);
+
+ n = (a-b) / (a+b);
+ fdf = (double)5.0 / (double)4.0;
+ fde = ((double)15.0 / (double)8.0);
+
+ esq = ((a*a)-(b*b)) / (a*a);
+
+
+ phix = ((N-N0)/(a*F0)) + phi0;
+
+ tmp = (double)1.0 - (esq * sin(phix) * sin(phix));
+ nu = a * F0 * pow(tmp,(double)-0.5);
+ rho = a * F0 * ((double)1.0 - esq) * pow(tmp,(double)-1.5);
+ etasq = (nu / rho) - (double)1.0;
+
+ M = (double)-1e20;
+
+ while(N-N0-M > (double)0.000001)
+ {
+ nphi = phix;
+
+ tmp = (double)1.0 + n + (fdf * n * n) + (fdf * n * n * n);
+ tmp *= (nphi - phi0);
+ tmp2 = (double)3.0*n + (double)3.0*n*n +
+ ((double)21./(double)8.)*n*n*n;
+ tmp2 *= (sin(nphi-phi0) * cos(nphi+phi0));
+ tmp -= tmp2;
+
+
+ tmp2 = ((fde*n*n) + (fde*n*n*n)) * sin((double)2.0 * (nphi-phi0));
+ tmp2 *= cos((double)2.0 * (nphi+phi0));
+ tmp += tmp2;
+
+ tmp2 = ((double)35.0/(double)24.0) * n * n * n;
+ tmp2 *= sin((double)3.0 * (nphi-phi0));
+ tmp2 *= cos((double)3.0 * (nphi+phi0));
+ tmp -= tmp2;
+
+ M = b * F0 * tmp;
+
+ if(N-N0-M > (double)0.000001)
+ phix = ((N-N0-M)/(a*F0)) + nphi;
+ }
+
+
+ VII = tan(nphi) / ((double)2.0 * rho * nu);
+
+ tmp = (double)5.0 + (double)3.0 * tan(nphi) * tan(nphi) + etasq;
+ tmp -= (double)9.0 * tan(nphi) * tan(nphi) * etasq;
+ VIII = (tan(nphi)*tmp) / ((double)24.0 * rho * nu*nu*nu);
+
+ tmp = (double)61.0 + (double)90.0 * tan(nphi) * tan(nphi);
+ tmp += (double)45.0 * pow(tan(nphi),(double)4.0);
+ IX = tan(nphi) / ((double)720.0 * rho * pow(nu,(double)5.0)) * tmp;
+
+ X = (double)1.0 / (cos(nphi) * nu);
+
+ tmp = (nu / rho) + (double)2.0 * tan(nphi) * tan(nphi);
+ XI = ((double)1.0 / (cos(nphi) * (double)6.0 * nu*nu*nu)) * tmp;
+
+ tmp = (double)5.0 + (double)28.0 * tan(nphi)*tan(nphi);
+ tmp += (double)24.0 * pow(tan(nphi),(double)4.0);
+ XII = ((double)1.0 / ((double)120.0 * pow(nu,(double)5.0) * cos(nphi)))
+ * tmp;
+
+ tmp = (double)61.0 + (double)662.0 * tan(nphi) * tan(nphi);
+ tmp += (double)1320.0 * pow(tan(nphi),(double)4.0);
+ tmp += (double)720.0 * pow(tan(nphi),(double)6.0);
+ XIIA = ((double)1.0 / (cos(nphi) * (double)5040.0 * pow(nu,(double)7.0)))
+ * tmp;
+
+ *phi = nphi - VII*pow((E-E0),(double)2.0) + VIII*pow((E-E0),(double)4.0) -
+ IX*pow((E-E0),(double)6.0);
+
+ *lambda = lambda0 + X*(E-E0) - XI*pow((E-E0),(double)3.0) +
+ XII*pow((E-E0),(double)5.0) - XIIA*pow((E-E0),(double)7.0);
+
+ *phi = GPS_Math_Rad_To_Deg(*phi);
+ *lambda = GPS_Math_Rad_To_Deg(*lambda);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_NGENToAiry1830LatLon **************************************
+**
+** Convert to UK Ordnance Survey National Grid Eastings and Northings to
+** Airy 1830 datum latitude and longitude
+**
+** @param [r] E [double] NG easting (metres)
+** @param [r] N [double] NG northing (metres)
+** @param [w] phi [double *] Airy latitude (deg)
+** @param [w] lambda [double *] Airy longitude (deg)
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_NGENToAiry1830LatLon(double E, double N, double *phi,
+ double *lambda)
+{
+ double N0 = -100000;
+ double E0 = 400000;
+ double F0 = 0.9996012717;
+ double phi0 = 49.;
+ double lambda0 = -2.;
+ double a = 6377563.396;
+ double b = 6356256.910;
+
+ GPS_Math_EN_To_LatLon(E,N,phi,lambda,N0,E0,phi0,lambda0,F0,a,b);
+
+ return;
+}
+
+
+
+/* @func GPS_Math_INGENToAiry1830MLatLon **************************************
+**
+** Convert Irish National Grid Eastings and Northings to modified
+** Airy 1830 datum latitude and longitude
+**
+** @param [r] E [double] ING easting (metres)
+** @param [r] N [double] ING northing (metres)
+** @param [w] phi [double *] modified Airy latitude (deg)
+** @param [w] lambda [double *] modified Airy longitude (deg)
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_INGENToAiry1830MLatLon(double E, double N, double *phi,
+ double *lambda)
+{
+ double N0 = 250000;
+ double E0 = 200000;
+ double F0 = 1.000035;
+ double phi0 = 53.5;
+ double lambda0 = -8.;
+ double a = 6377340.189;
+ double b = 6356034.447;
+
+ GPS_Math_EN_To_LatLon(E,N,phi,lambda,N0,E0,phi0,lambda0,F0,a,b);
+
+ return;
+}
+
+
+
+/* @func GPS_Math_EN_To_UKOSNG_Map *************************************
+**
+** Convert Airy 1830 eastings and northings to Ordnance Survey map
+** two letter code plus modified eastings and northings
+**
+** @param [r] E [double] NG easting (metres)
+** @param [r] N [double] NG northing (metres)
+** @param [w] mE [double *] modified easting (metres)
+** @param [w] mN [double *] modified northing (metres)
+** @param [w] map [char *] map code
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_Math_EN_To_UKOSNG_Map(double E, double N, double *mE,
+ double *mN, char *map)
+{
+ int32 t;
+ int32 idx;
+
+ if(E>=(double)700000. || E<(double)0.0 || N<(double)0.0 ||
+ N>=(double)1300000.0)
+ return 0;
+
+ idx = ((int32)N/100000)*7 + (int32)E/100000;
+ (void) strcpy(map,UKNG[idx]);
+
+ t = ((int32)E / 100000) * 100000;
+ *mE = E - (double)t;
+
+ t = ((int32)N / 100000) * 100000;
+ *mN = N - (double)t;
+
+ return 1;
+}
+
+
+
+/* @func GPS_Math_UKOSNG_Map_To_EN *************************************
+**
+** Convert Ordnance Survey map eastings and northings plus
+** two letter code to Airy 1830 eastings and northings
+**
+** @param [w] map [char *] map code
+** @param [r] mapE [double] easting (metres)
+** @param [r] mapN [double] northing (metres)
+** @param [w] E [double *] full Airy easting (metres)
+** @param [w] N [double *] full Airy northing (metres)
+
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_Math_UKOSNG_Map_To_EN(char *map, double mapE, double mapN, double *E,
+ double *N)
+{
+ int32 t;
+ int32 idx;
+
+ if(mapE>=(double)100000.0 || mapE<(double)0.0 || mapN<(double)0.0 ||
+ mapN>(double)100000.0)
+ return 0;
+
+ idx=0;
+ while(*UKNG[idx])
+ {
+ if(!strcmp(UKNG[idx],map)) break;
+ ++idx;
+ }
+ if(!*UKNG[idx])
+ return 0;
+
+
+ t = (idx / 7) * 100000;
+ *N = mapN + (double)t;
+
+ t = (idx % 7) * 100000;
+ *E = mapE + (double)t;
+
+ return 1;
+}
+
+
+
+/* @func GPS_Math_Molodensky *******************************************
+**
+** Transform one datum to another
+**
+** @param [r] Sphi [double] source latitude (deg)
+** @param [r] Slam [double] source longitude (deg)
+** @param [r] SH [double] source height (metres)
+** @param [r] Sa [double] source semi-major axis (metres)
+** @param [r] Sif [double] source inverse flattening
+** @param [w] Dphi [double *] dest latitude (deg)
+** @param [w] Dlam [double *] dest longitude (deg)
+** @param [w] DH [double *] dest height (metres)
+** @param [r] Da [double] dest semi-major axis (metres)
+** @param [r] Dif [double] dest inverse flattening
+** @param [r] dx [double] dx
+** @param [r] dy [double] dy
+** @param [r] dz [double] dz
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Molodensky(double Sphi, double Slam, double SH, double Sa,
+ double Sif, double *Dphi, double *Dlam,
+ double *DH, double Da, double Dif, double dx,
+ double dy, double dz)
+{
+ double Sf;
+ double Df;
+ double esq;
+ double bda;
+ double da;
+ double df;
+ double N;
+ double M;
+ double tmp;
+ double tmp2;
+ double dphi;
+ double dlambda;
+ double dheight;
+ double phis;
+ double phic;
+ double lams;
+ double lamc;
+
+ Sf = (double)1.0 / Sif;
+ Df = (double)1.0 / Dif;
+
+ esq = (double)2.0*Sf - pow(Sf,(double)2.0);
+ bda = (double)1.0 - Sf;
+ Sphi = GPS_Math_Deg_To_Rad(Sphi);
+ Slam = GPS_Math_Deg_To_Rad(Slam);
+
+ da = Da - Sa;
+ df = Df - Sf;
+
+ phis = sin(Sphi);
+ phic = cos(Sphi);
+ lams = sin(Slam);
+ lamc = cos(Slam);
+
+ N = Sa / sqrt((double)1.0 - esq*pow(phis,(double)2.0));
+
+ tmp = ((double)1.0-esq) /pow(((double)1.0-esq*pow(phis,(double)2.0)),1.5);
+ M = Sa * tmp;
+
+ tmp = df * ((M/bda)+N*bda) * phis * phic;
+ tmp2 = da * N * esq * phis * phic / Sa;
+ tmp2 += ((-dx*phis*lamc-dy*phis*lams) + dz*phic);
+ dphi = (tmp2 + tmp) / (M + SH);
+
+ dlambda = (-dx*lams+dy*lamc) / ((N+SH)*phic);
+
+ dheight = dx*phic*lamc + dy*phic*lams + dz*phis - da*(Sa/N) +
+ df*bda*N*phis*phis;
+
+ *Dphi = Sphi + dphi;
+ *Dlam = Slam + dlambda;
+ *DH = SH + dheight;
+
+ *Dphi = GPS_Math_Rad_To_Deg(*Dphi);
+ *Dlam = GPS_Math_Rad_To_Deg(*Dlam);
+
+ return;
+}
+
+
+
+/* @func GPS_Math_Known_Datum_To_WGS84_M **********************************
+**
+** Transform datum to WGS84 using Molodensky
+**
+** @param [r] Sphi [double] source latitude (deg)
+** @param [r] Slam [double] source longitude (deg)
+** @param [r] SH [double] source height (metres)
+** @param [w] Dphi [double *] dest latitude (deg)
+** @param [w] Dlam [double *] dest longitude (deg)
+** @param [w] DH [double *] dest height (metres)
+** @param [r] n [int32] datum number from GPS_Datum structure
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Known_Datum_To_WGS84_M(double Sphi, double Slam, double SH,
+ double *Dphi, double *Dlam, double *DH,
+ int32 n)
+{
+ double Sa;
+ double Sif;
+ double Da;
+ double Dif;
+ double x;
+ double y;
+ double z;
+ int32 idx;
+
+ Da = (double) 6378137.0;
+ Dif = (double) 298.257223563;
+
+ idx = GPS_Datum[n].ellipse;
+ Sa = GPS_Ellipse[idx].a;
+ Sif = GPS_Ellipse[idx].invf;
+ x = GPS_Datum[n].dx;
+ y = GPS_Datum[n].dy;
+ z = GPS_Datum[n].dz;
+
+ GPS_Math_Molodensky(Sphi,Slam,SH,Sa,Sif,Dphi,Dlam,DH,Da,Dif,x,y,z);
+
+ return;
+}
+
+
+
+/* @func GPS_Math_WGS84_To_Known_Datum_M ********************************
+**
+** Transform WGS84 to other datum using Molodensky
+**
+** @param [r] Sphi [double] source latitude (deg)
+** @param [r] Slam [double] source longitude (deg)
+** @param [r] SH [double] source height (metres)
+** @param [w] Dphi [double *] dest latitude (deg)
+** @param [w] Dlam [double *] dest longitude (deg)
+** @param [w] DH [double *] dest height (metres)
+** @param [r] n [int32] datum number from GPS_Datum structure
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_WGS84_To_Known_Datum_M(double Sphi, double Slam, double SH,
+ double *Dphi, double *Dlam, double *DH,
+ int32 n)
+{
+ double Sa;
+ double Sif;
+ double Da;
+ double Dif;
+ double x;
+ double y;
+ double z;
+ int32 idx;
+
+ Sa = (double) 6378137.0;
+ Sif = (double) 298.257223563;
+
+ idx = GPS_Datum[n].ellipse;
+ Da = GPS_Ellipse[idx].a;
+ Dif = GPS_Ellipse[idx].invf;
+ x = -GPS_Datum[n].dx;
+ y = -GPS_Datum[n].dy;
+ z = -GPS_Datum[n].dz;
+
+ GPS_Math_Molodensky(Sphi,Slam,SH,Sa,Sif,Dphi,Dlam,DH,Da,Dif,x,y,z);
+
+ return;
+}
+
+
+
+/* @func GPS_Math_Known_Datum_To_WGS84_C **********************************
+**
+** Transform datum to WGS84 using Cartesian coordinates
+**
+** @param [r] Sphi [double] source latitude (deg)
+** @param [r] Slam [double] source longitude (deg)
+** @param [r] SH [double] source height (metres)
+** @param [w] Dphi [double *] dest latitude (deg)
+** @param [w] Dlam [double *] dest longitude (deg)
+** @param [w] DH [double *] dest height (metres)
+** @param [r] n [int32] datum number from GPS_Datum structure
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Known_Datum_To_WGS84_C(double Sphi, double Slam, double SH,
+ double *Dphi, double *Dlam, double *DH,
+ int32 n)
+{
+ double Sa;
+ double Sif;
+ double Sb;
+ double Da;
+ double Dif;
+ double Db;
+ double x;
+ double y;
+ double z;
+ int32 idx;
+ double sx;
+ double sy;
+ double sz;
+
+ Da = (double) 6378137.0;
+ Dif = (double) 298.257223563;
+ Db = Da - (Da / Dif);
+
+ idx = GPS_Datum[n].ellipse;
+ Sa = GPS_Ellipse[idx].a;
+ Sif = GPS_Ellipse[idx].invf;
+ Sb = Sa - (Sa / Sif);
+
+ x = GPS_Datum[n].dx;
+ y = GPS_Datum[n].dy;
+ z = GPS_Datum[n].dz;
+
+ GPS_Math_LatLonH_To_XYZ(Sphi,Slam,SH,&sx,&sy,&sz,Sa,Sb);
+ sx += x;
+ sy += y;
+ sz += z;
+
+ GPS_Math_XYZ_To_LatLonH(Dphi,Dlam,DH,sx,sy,sz,Da,Db);
+
+ return;
+}
+
+
+
+/* @func GPS_Math_WGS84_To_Known_Datum_C ********************************
+**
+** Transform WGS84 to other datum using Cartesian coordinates
+**
+** @param [r] Sphi [double] source latitude (deg)
+** @param [r] Slam [double] source longitude (deg)
+** @param [r] SH [double] source height (metres)
+** @param [w] Dphi [double *] dest latitude (deg)
+** @param [w] Dlam [double *] dest longitude (deg)
+** @param [w] DH [double *] dest height (metres)
+** @param [r] n [int32] datum number from GPS_Datum structure
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_WGS84_To_Known_Datum_C(double Sphi, double Slam, double SH,
+ double *Dphi, double *Dlam, double *DH,
+ int32 n)
+{
+ double Sa;
+ double Sif;
+ double Da;
+ double Dif;
+ double x;
+ double y;
+ double z;
+ int32 idx;
+ double Sb;
+ double Db;
+ double dx;
+ double dy;
+ double dz;
+
+ Sa = (double) 6378137.0;
+ Sif = (double) 298.257223563;
+ Sb = Sa - (Sa / Sif);
+
+ idx = GPS_Datum[n].ellipse;
+ Da = GPS_Ellipse[idx].a;
+ Dif = GPS_Ellipse[idx].invf;
+ Db = Da - (Da / Dif);
+
+ x = -GPS_Datum[n].dx;
+ y = -GPS_Datum[n].dy;
+ z = -GPS_Datum[n].dz;
+
+ GPS_Math_LatLonH_To_XYZ(Sphi,Slam,SH,&dx,&dy,&dz,Sa,Sb);
+ dx += x;
+ dy += y;
+ dz += z;
+
+ GPS_Math_XYZ_To_LatLonH(Dphi,Dlam,DH,dx,dy,dz,Da,Db);
+
+ return;
+}
+
+
+
+/* @func GPS_Math_Known_Datum_To_Known_Datum_M *************************
+**
+** Transform WGS84 to other datum using Molodensky
+**
+** @param [r] Sphi [double] source latitude (deg)
+** @param [r] Slam [double] source longitude (deg)
+** @param [r] SH [double] source height (metres)
+** @param [w] Dphi [double *] dest latitude (deg)
+** @param [w] Dlam [double *] dest longitude (deg)
+** @param [w] DH [double *] dest height (metres)
+** @param [r] n1 [int32] source datum number from GPS_Datum structure
+** @param [r] n2 [int32] dest datum number from GPS_Datum structure
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Known_Datum_To_Known_Datum_M(double Sphi, double Slam, double SH,
+ double *Dphi, double *Dlam,
+ double *DH, int32 n1, int32 n2)
+{
+ double Sa;
+ double Sif;
+ double Da;
+ double Dif;
+ double x1;
+ double y1;
+ double z1;
+ double x2;
+ double y2;
+ double z2;
+ double x;
+ double y;
+ double z;
+
+ int32 idx1;
+ int32 idx2;
+
+
+ idx1 = GPS_Datum[n1].ellipse;
+ Sa = GPS_Ellipse[idx1].a;
+ Sif = GPS_Ellipse[idx1].invf;
+ x1 = GPS_Datum[n1].dx;
+ y1 = GPS_Datum[n1].dy;
+ z1 = GPS_Datum[n1].dz;
+
+ idx2 = GPS_Datum[n2].ellipse;
+ Da = GPS_Ellipse[idx2].a;
+ Dif = GPS_Ellipse[idx2].invf;
+ x2 = GPS_Datum[n2].dx;
+ y2 = GPS_Datum[n2].dy;
+ z2 = GPS_Datum[n2].dz;
+
+ x = -(x2-x1);
+ y = -(y2-y1);
+ z = -(z2-z1);
+
+ GPS_Math_Molodensky(Sphi,Slam,SH,Sa,Sif,Dphi,Dlam,DH,Da,Dif,x,y,z);
+
+ return;
+}
+
+
+
+/* @func GPS_Math_Known_Datum_To_Known_Datum_C *************************
+**
+** Transform known datum to other datum using Cartesian coordinates
+**
+** @param [r] Sphi [double] source latitude (deg)
+** @param [r] Slam [double] source longitude (deg)
+** @param [r] SH [double] source height (metres)
+** @param [w] Dphi [double *] dest latitude (deg)
+** @param [w] Dlam [double *] dest longitude (deg)
+** @param [w] DH [double *] dest height (metres)
+** @param [r] n1 [int32] source datum number from GPS_Datum structure
+** @param [r] n2 [int32] dest datum number from GPS_Datum structure
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Known_Datum_To_Known_Datum_C(double Sphi, double Slam, double SH,
+ double *Dphi, double *Dlam,
+ double *DH, int32 n1, int32 n2)
+{
+ double Sa;
+ double Sif;
+ double Da;
+ double Dif;
+ double x1;
+ double y1;
+ double z1;
+ double x2;
+ double y2;
+ double z2;
+
+ int32 idx1;
+ int32 idx2;
+
+ double Sb;
+ double Db;
+ double dx;
+ double dy;
+ double dz;
+
+ idx1 = GPS_Datum[n1].ellipse;
+ Sa = GPS_Ellipse[idx1].a;
+ Sif = GPS_Ellipse[idx1].invf;
+ Sb = Sa - (Sa / Sif);
+
+ x1 = GPS_Datum[n1].dx;
+ y1 = GPS_Datum[n1].dy;
+ z1 = GPS_Datum[n1].dz;
+
+ idx2 = GPS_Datum[n2].ellipse;
+ Da = GPS_Ellipse[idx2].a;
+ Dif = GPS_Ellipse[idx2].invf;
+ Db = Da - (Da / Dif);
+
+ x2 = GPS_Datum[n2].dx;
+ y2 = GPS_Datum[n2].dy;
+ z2 = GPS_Datum[n2].dz;
+
+ GPS_Math_LatLonH_To_XYZ(Sphi,Slam,SH,&dx,&dy,&dz,Sa,Sb);
+ dx += -(x2-x1);
+ dy += -(y2-y1);
+ dz += -(z2-z1);
+
+ GPS_Math_XYZ_To_LatLonH(Dphi,Dlam,DH,dx,dy,dz,Da,Db);
+
+ return;
+}
+
+
+
+/* @func GPS_Math_WGS84_To_UKOSMap_M ***********************************
+**
+** Convert WGS84 lat/lon to Ordnance survey map code and easting and
+** northing. Uses Molodensky
+**
+** @param [r] lat [double] WGS84 latitude (deg)
+** @param [r] lon [double] WGS84 longitude (deg)
+** @param [w] mE [double *] map easting (metres)
+** @param [w] mN [double *] map northing (metres)
+** @param [w] map [char *] map two letter code
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_Math_WGS84_To_UKOSMap_M(double lat, double lon, double *mE,
+ double *mN, char *map)
+{
+ double alat;
+ double alon;
+ double aht;
+ double aE;
+ double aN;
+
+
+ GPS_Math_WGS84_To_Known_Datum_M(lat,lon,30,&alat,&alon,&aht,86);
+
+ GPS_Math_Airy1830LatLonToNGEN(alat,alon,&aE,&aN);
+
+ if(!GPS_Math_EN_To_UKOSNG_Map(aE,aN,mE,mN,map))
+ return 0;
+
+ return 1;
+}
+
+
+
+/* @func GPS_Math_UKOSMap_To_WGS84_M ***********************************
+**
+** Transform UK Ordnance survey map position to WGS84 lat/lon
+** Uses Molodensky transformation
+**
+** @param [r] map [char *] map two letter code
+** @param [r] mE [double] map easting (metres)
+** @param [r] mN [double] map northing (metres)
+** @param [w] lat [double *] WGS84 latitude (deg)
+** @param [w] lon [double *] WGS84 longitude (deg)
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_Math_UKOSMap_To_WGS84_M(char *map, double mE, double mN,
+ double *lat, double *lon)
+{
+ double E;
+ double N;
+ double alat;
+ double alon;
+ double ht;
+
+ if(!GPS_Math_UKOSNG_Map_To_EN(map,mE,mN,&E,&N))
+ return 0;
+
+ GPS_Math_NGENToAiry1830LatLon(E,N,&alat,&alon);
+
+ GPS_Math_Known_Datum_To_WGS84_M(alat,alon,0,lat,lon,&ht,78);
+
+ return 1;
+}
+
+
+
+/* @func GPS_Math_WGS84_To_UKOSMap_C ***********************************
+**
+** Convert WGS84 lat/lon to Ordnance survey map code and easting and
+** northing. Uses cartesian transformation
+**
+** @param [r] lat [double] WGS84 latitude (deg)
+** @param [r] lon [double] WGS84 longitude (deg)
+** @param [w] mE [double *] map easting (metres)
+** @param [w] mN [double *] map northing (metres)
+** @param [w] map [char *] map two letter code
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_Math_WGS84_To_UKOSMap_C(double lat, double lon, double *mE,
+ double *mN, char *map)
+{
+ double alat;
+ double alon;
+ double aht;
+ double aE;
+ double aN;
+
+
+ GPS_Math_WGS84_To_Known_Datum_C(lat,lon,30,&alat,&alon,&aht,86);
+
+ GPS_Math_Airy1830LatLonToNGEN(alat,alon,&aE,&aN);
+
+ if(!GPS_Math_EN_To_UKOSNG_Map(aE,aN,mE,mN,map))
+ return 0;
+
+ return 1;
+}
+
+
+
+/* @func GPS_Math_UKOSMap_To_WGS84_C ***********************************
+**
+** Transform UK Ordnance survey map position to WGS84 lat/lon
+** Uses cartesian transformation
+**
+** @param [r] map [char *] map two letter code
+** @param [r] mE [double] map easting (metres)
+** @param [r] mN [double] map northing (metres)
+** @param [w] lat [double *] WGS84 latitude (deg)
+** @param [w] lon [double *] WGS84 longitude (deg)
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_Math_UKOSMap_To_WGS84_C(char *map, double mE, double mN,
+ double *lat, double *lon)
+{
+ double E;
+ double N;
+ double alat;
+ double alon;
+ double ht;
+
+ if(!GPS_Math_UKOSNG_Map_To_EN(map,mE,mN,&E,&N))
+ return 0;
+
+ GPS_Math_NGENToAiry1830LatLon(E,N,&alat,&alon);
+
+ GPS_Math_Known_Datum_To_WGS84_C(alat,alon,0,lat,lon,&ht,78);
+
+ return 1;
+}
+
+
+/* @funcstatic GPS_Math_LatLon_To_UTM_Param *****************************
+**
+** Transform NAD33
+**
+** @param [r] lat [double] NAD latitude (deg)
+** @param [r] lon [double] NAD longitude (deg)
+** @param [w] zone [int32 *] zone number
+** @param [w] zc [char *] zone character
+** @param [w] Mc [double *] central meridian
+** @param [w] E0 [double *] false easting
+** @param [w] N0 [double *] false northing
+** @param [w] F0 [double *] scale factor
+**
+** @return [int32] success
+************************************************************************/
+static int32 GPS_Math_LatLon_To_UTM_Param(double lat, double lon, int32 *zone,
+ char *zc, double *Mc, double *E0,
+ double *N0, double *F0)
+{
+ int32 ilon;
+ int32 ilat;
+ int32 psign;
+ int32 lsign;
+
+ if(lat >= (double)84.0 || lat < (double)-80.0)
+ return 0;
+
+ psign = lsign = 0;
+ if(lon < (double)0.0)
+ lsign=1;
+ if(lat < (double)0.0)
+ psign=1;
+
+ ilon = abs((int32)lon);
+ ilat = abs((int32)lat);
+
+ if(!lsign)
+ {
+ *zone = 31 + (ilon / 6);
+ *Mc = (double)((ilon / 6) * 6 + 3);
+ }
+ else
+ {
+ *zone = 30 - (ilon / 6);
+ *Mc = -(double)((ilon / 6) * 6 + 3);
+ }
+
+ if(!psign)
+ {
+ *zc = 'N' + ilat / 8;
+ if(*zc > 'N') ++*zc;
+ }
+ else
+ {
+ *zc = 'M' - (ilat / 8);
+ if(*zc <= 'I') --*zc;
+ }
+
+
+ if(lat>=(double)56.0 && lat<(double)64.0 && lon>=(double)3.0 &&
+ lon<(double)12.0)
+ {
+ *zone = 32;
+ *zc = 'V';
+ *Mc = (double)9.0;
+ }
+
+ if(*zc=='X' && lon>=(double)0.0 && lon<(double)42.0)
+ {
+ if(lon<(double)9.0)
+ {
+ *zone = 31;
+ *Mc = (double)3.0;
+ }
+ else if(lon<(double)21.0)
+ {
+ *zone = 33;
+ *Mc = (double)15.0;
+ }
+ else if(lon<(double)33.0)
+ {
+ *zone = 35;
+ *Mc = (double)27.0;
+ }
+ else
+ {
+ *zone = 37;
+ *Mc = (double)39.0;
+ }
+ }
+
+ if(!psign)
+ *N0 = (double)0.0;
+ else
+ *N0 = (double)10000000;
+
+ *E0 = (double)500000;
+ *F0 = (double)0.9996;
+
+ return 1;
+}
+
+
+
+/* @func GPS_Math_NAD83_To_UTM_EN **************************************
+**
+** Transform NAD33 lat/lon to UTM zone, easting and northing
+**
+** @param [r] lat [double] NAD latitude (deg)
+** @param [r] lon [double] NAD longitude (deg)
+** @param [w] E [double *] easting (metres)
+** @param [w] N [double *] northing (metres)
+** @param [w] zone [int32 *] zone number
+** @param [w] zc [char *] zone character
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_Math_NAD83_To_UTM_EN(double lat, double lon, double *E,
+ double *N, int32 *zone, char *zc)
+{
+ double phi0;
+ double lambda0;
+ double N0;
+ double E0;
+ double F0;
+ double a;
+ double b;
+
+ if(!GPS_Math_LatLon_To_UTM_Param(lat,lon,zone,zc,&lambda0,&E0,
+ &N0,&F0))
+ return 0;
+
+ phi0 = (double)0.0;
+
+ a = (double) GPS_Ellipse[21].a;
+ b = a - (a/GPS_Ellipse[21].invf);
+
+ GPS_Math_LatLon_To_EN(E,N,lat,lon,N0,E0,phi0,lambda0,F0,a,b);
+
+ return 1;
+}
+
+
+
+/* @func GPS_Math_WGS84_To_UTM_EN **************************************
+**
+** Transform WGS84 lat/lon to UTM zone, easting and northing
+**
+** @param [r] lat [double] WGS84 latitude (deg)
+** @param [r] lon [double] WGS84 longitude (deg)
+** @param [w] E [double *] easting (metres)
+** @param [w] N [double *] northing (metres)
+** @param [w] zone [int32 *] zone number
+** @param [w] zc [char *] zone character
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_Math_WGS84_To_UTM_EN(double lat, double lon, double *E,
+ double *N, int32 *zone, char *zc)
+{
+ double phi;
+ double lambda;
+ double H;
+
+ GPS_Math_WGS84_To_Known_Datum_M(lat,lon,0,&phi,&lambda,&H,77);
+ if(!GPS_Math_NAD83_To_UTM_EN(phi,lambda,E,N,zone,zc))
+ return 0;
+
+ return 1;
+}
+
+
+
+/* @funcstatic GPS_Math_UTM_Param_To_Mc ********************************
+**
+** Convert UTM zone and zone character to central meridian value.
+** Also return false eastings, northings and scale factor
+**
+** @param [w] zone [int32] zone number
+** @param [w] zc [char] zone character
+** @param [w] Mc [double *] central meridian
+** @param [w] E0 [double *] false easting
+** @param [w] N0 [double *] false northing
+** @param [w] F0 [double *] scale factor
+**
+** @return [int32] success
+************************************************************************/
+static int32 GPS_Math_UTM_Param_To_Mc(int32 zone, char zc, double *Mc,
+ double *E0, double *N0, double *F0)
+{
+
+ if(zone>60 || zone<0 || zc<'C' || zc>'X')
+ return 0;
+
+ if(zone > 30)
+ *Mc = (double)((zone-31)*6) + (double)3.0;
+ else
+ *Mc = (double) -(((30-zone)*6)+3);
+
+ if(zone==32 && zc=='V')
+ *Mc = (double)9.0;
+
+ if(zone==31 && zc=='X')
+ *Mc = (double)3.0;
+ if(zone==33 && zc=='X')
+ *Mc = (double)15.0;
+ if(zone==35 && zc=='X')
+ *Mc = (double)27.0;
+ if(zone==37 && zc=='X')
+ *Mc = (double)39.0;
+
+ if(zc>'M')
+ *N0 = (double)0.0;
+ else
+ *N0 = (double)10000000;
+
+ *E0 = (double)500000;
+ *F0 = (double)0.9996;
+
+ return 1;
+}
+
+
+
+/* @func GPS_Math_UTM_EN_To_NAD83 **************************************
+**
+** Transform UTM zone, easting and northing to NAD83 lat/lon
+**
+** @param [r] lat [double *] NAD latitude (deg)
+** @param [r] lon [double *] NAD longitude (deg)
+** @param [w] E [double] easting (metres)
+** @param [w] N [double] northing (metres)
+** @param [w] zone [int32] zone number
+** @param [w] zc [char] zone character
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_Math_UTM_EN_To_NAD83(double *lat, double *lon, double E,
+ double N, int32 zone, char zc)
+{
+ double phi0;
+ double lambda0;
+ double N0;
+ double E0;
+ double F0;
+ double a;
+ double b;
+
+ if(!GPS_Math_UTM_Param_To_Mc(zone,zc,&lambda0,&E0,&N0,&F0))
+ return 0;
+
+ phi0 = (double)0.0;
+
+ a = (double) GPS_Ellipse[21].a;
+ b = a - (a/GPS_Ellipse[21].invf);
+
+ GPS_Math_EN_To_LatLon(E,N,lat,lon,N0,E0,phi0,lambda0,F0,a,b);
+
+ return 1;
+}
+
+
+
+/* @func GPS_Math_UTM_EN_To_WGS84 **************************************
+**
+** Transform UTM zone, easting and northing to WGS84 lat/lon
+**
+** @param [w] lat [double *] WGS84 latitude (deg)
+** @param [r] lon [double *] WGS84 longitude (deg)
+** @param [w] E [double] easting (metres)
+** @param [w] N [double] northing (metres)
+** @param [w] zone [int32] zone number
+** @param [w] zc [char] zone character
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_Math_UTM_EN_To_WGS84(double *lat, double *lon, double E,
+ double N, int32 zone, char zc)
+{
+ double phi;
+ double lambda;
+ double H;
+
+ if(!GPS_Math_UTM_EN_To_NAD83(&phi,&lambda,E,N,zone,zc))
+ return 0;
+
+
+ GPS_Math_Known_Datum_To_WGS84_M(phi,lambda,0,lat,lon,&H,77);
+
+ return 1;
+}
--- /dev/null
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef gpsmath_h
+#define gpsmath_h
+
+
+#include "gps.h"
+
+#define GPS_PI 3.141592653589
+#define GPS_FLTMIN 1.75494351E-38
+#define GPS_FLTMAX 3.402823466E+38
+
+
+double GPS_Math_Deg_To_Rad(double v);
+double GPS_Math_Rad_To_Deg(double v);
+
+double GPS_Math_Metres_To_Feet(double v);
+double GPS_Math_Feet_To_Metres(double v);
+
+int32 GPS_Math_Deg_To_Semi(double v);
+double GPS_Math_Semi_To_Deg(int32 v);
+
+time_t GPS_Math_Utime_To_Gtime(time_t v);
+time_t GPS_Math_Gtime_To_Utime(time_t v);
+
+void GPS_Math_Deg_To_DegMin(double v, int32 *d, double *m);
+void GPS_Math_DegMin_To_Deg(int32 d, double m, double *deg);
+void GPS_Math_Deg_To_DegMinSec(double v, int32 *d, int32 *m, double *s);
+void GPS_Math_DegMinSec_To_Deg(int32 d, int32 m, double s, double *deg);
+
+
+void GPS_Math_Airy1830LatLonToNGEN(double phi, double lambda, double *E,
+ double *N);
+void GPS_Math_Airy1830M_LatLonToINGEN(double phi, double lambda, double *E,
+ double *N);
+int32 GPS_Math_EN_To_UKOSNG_Map(double E, double N, double *mE,
+ double *mN, char *map);
+int32 GPS_Math_UKOSNG_Map_To_EN(char *map, double mapE, double mapN,
+ double *E, double *N);
+
+void GPS_Math_LatLonH_To_XYZ(double phi, double lambda, double H,
+ double *x, double *y, double *z,
+ double a, double b);
+void GPS_Math_XYZ_To_LatLonH(double *phi, double *lambda, double *H,
+ double x, double y, double z,
+ double a, double b);
+
+void GPS_Math_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double N0, double E0,
+ double phi0, double lambda0,
+ double F0, double a, double b);
+void GPS_Math_LatLon_To_EN(double *E, double *N, double phi,
+ double lambda, double N0, double E0,
+ double phi0, double lambda0,
+ double F0, double a, double b);
+
+void GPS_Math_NGENToAiry1830LatLon(double E, double N, double *phi,
+ double *lambda);
+void GPS_Math_INGENToAiry1830MLatLon(double E, double N, double *phi,
+ double *lambda);
+
+
+void GPS_Math_Airy1830LatLonH_To_XYZ(double phi, double lambda, double H,
+ double *x, double *y, double *z);
+void GPS_Math_WGS84LatLonH_To_XYZ(double phi, double lambda, double H,
+ double *x, double *y, double *z);
+void GPS_Math_XYZ_To_Airy1830LatLonH(double *phi, double *lambda, double *H,
+ double x, double y, double z);
+void GPS_Math_XYZ_To_WGS84LatLonH(double *phi, double *lambda, double *H,
+ double x, double y, double z);
+
+void GPS_Math_Molodensky(double Sphi, double Slam, double SH, double Sa,
+ double Sif, double *Dphi, double *Dlam,
+ double *DH, double Da, double Dif, double dx,
+ double dy, double dz);
+void GPS_Math_Known_Datum_To_WGS84_M(double Sphi, double Slam, double SH,
+ double *Dphi, double *Dlam, double *DH,
+ int32 n);
+void GPS_Math_WGS84_To_Known_Datum_M(double Sphi, double Slam, double SH,
+ double *Dphi, double *Dlam, double *DH,
+ int32 n);
+void GPS_Math_Known_Datum_To_WGS84_C(double Sphi, double Slam, double SH,
+ double *Dphi, double *Dlam, double *DH,
+ int32 n);
+void GPS_Math_WGS84_To_Known_Datum_C(double Sphi, double Slam, double SH,
+ double *Dphi, double *Dlam, double *DH,
+ int32 n);
+
+void GPS_Math_Known_Datum_To_Known_Datum_M(double Sphi, double Slam, double SH,
+ double *Dphi, double *Dlam,
+ double *DH, int32 n1, int32 n2);
+void GPS_Math_Known_Datum_To_Known_Datum_C(double Sphi, double Slam, double SH,
+ double *Dphi, double *Dlam,
+ double *DH, int32 n1, int32 n2);
+
+int32 GPS_Math_WGS84_To_UKOSMap_M(double lat, double lon, double *mE,
+ double *mN, char *map);
+int32 GPS_Math_UKOSMap_To_WGS84_M(char *map, double mE, double mN,
+ double *lat, double *lon);
+int32 GPS_Math_WGS84_To_UKOSMap_C(double lat, double lon, double *mE,
+ double *mN, char *map);
+int32 GPS_Math_UKOSMap_To_WGS84_C(char *map, double mE, double mN,
+ double *lat, double *lon);
+
+
+int32 GPS_Math_NAD83_To_UTM_EN(double lat, double lon, double *E,
+ double *N, int32 *zone, char *zc);
+int32 GPS_Math_WGS84_To_UTM_EN(double lat, double lon, double *E,
+ double *N, int32 *zone, char *zc);
+
+int32 GPS_Math_UTM_EN_To_WGS84(double *lat, double *lon, double E,
+ double N, int32 zone, char zc);
+int32 GPS_Math_UTM_EN_To_NAD83(double *lat, double *lon, double E,
+ double N, int32 zone, char zc);
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null
+/********************************************************************
+** @source JEEPS constructor and deconstructor functions
+**
+** @author Copyright (C) 1999,2000 Alan Bleasby
+** @version 1.0
+** @modified December 28th 1999 Alan Bleasby. First version
+** @modified June 29th 2000 Alan Bleasby. NMEA additions
+** @@
+**
+** This library is free software; you can redistribute it and/or
+** modify it under the terms of the GNU Library General Public
+** License as published by the Free Software Foundation; either
+** version 2 of the License, or (at your option) any later version.
+**
+** This library is distributed in the hope that it will be useful,
+** but WITHOUT ANY WARRANTY; without even the implied warranty of
+** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+** Library General Public License for more details.
+**
+** You should have received a copy of the GNU Library General Public
+** License along with this library; if not, write to the
+** Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+** Boston, MA 02111-1307, USA.
+********************************************************************/
+#include "gps.h"
+#include <stdlib.h>
+#include <errno.h>
+#include <stdio.h>
+#include <limits.h>
+
+
+/* @func GPS_Packet_New ***********************************************
+**
+** Packet constructor
+**
+** @return [GPS_PPacket] virgin packet
+**********************************************************************/
+
+GPS_PPacket GPS_Packet_New(void)
+{
+ GPS_PPacket ret;
+
+ if(!(ret=(GPS_PPacket )malloc(sizeof(GPS_OPacket))))
+ {
+ perror("malloc");
+ fprintf(stderr,"GPS_Packet_New: Insufficient memory");
+ fflush(stderr);
+ return NULL;
+ }
+
+ if(!(ret->data = (UC *)malloc(MAX_GPS_PACKET_SIZE*sizeof(UC))))
+ {
+ perror("malloc");
+ fprintf(stderr,"GPS_Packet_New: Insufficient data memory");
+ fflush(stderr);
+ return NULL;
+ }
+
+ ret->dle = ret->edle = DLE;
+ ret->etx = ETX;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Packet_Del ***********************************************
+**
+** Packet destructor
+**
+** @param [w] thys [GPS_PPacket *] packet to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Packet_Del(GPS_PPacket *thys)
+{
+ free((void *)(*thys)->data);
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+/* @func GPS_Pvt_New ***********************************************
+**
+** Pvt constructor
+**
+** @return [GPS_PPvt_Data] virgin pvt
+**********************************************************************/
+
+GPS_PPvt_Data GPS_Pvt_New(void)
+{
+ GPS_PPvt_Data ret;
+
+ if(!(ret=(GPS_PPvt_Data)malloc(sizeof(GPS_OPvt_Data))))
+ {
+ perror("malloc");
+ fprintf(stderr,"GPS_Pvt_New: Insufficient memory");
+ fflush(stderr);
+ return NULL;
+ }
+
+ return ret;
+}
+
+
+
+/* @func GPS_Pvt_Del ***********************************************
+**
+** Pvt destructor
+**
+** @param [w] thys [GPS_PPvt_Data *] pvt to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Pvt_Del(GPS_PPvt_Data *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+/* @func GPS_Almanac_New ***********************************************
+**
+** Almanac constructor
+**
+** @return [GPS_PAlmanac] virgin almanac
+**********************************************************************/
+
+GPS_PAlmanac GPS_Almanac_New(void)
+{
+ GPS_PAlmanac ret;
+
+ if(!(ret=(GPS_PAlmanac)malloc(sizeof(GPS_OAlmanac))))
+ {
+ perror("malloc");
+ fprintf(stderr,"GPS_Almanac_New: Insufficient memory");
+ fflush(stderr);
+ return NULL;
+ }
+
+ ret->svid=0xff;
+ ret->wn = -1;
+ ret->hlth=0xff;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Almanac_Del ***********************************************
+**
+** Almanac destructor
+**
+** @param [w] thys [GPS_PAlmanac *] almanac to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Almanac_Del(GPS_PAlmanac *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+/* @func GPS_Track_New ***********************************************
+**
+** Track constructor
+**
+** @return [GPS_PTrack] virgin track
+**********************************************************************/
+
+GPS_PTrack GPS_Track_New(void)
+{
+ GPS_PTrack ret;
+
+ if(!(ret=(GPS_PTrack)malloc(sizeof(GPS_OTrack))))
+ {
+ perror("malloc");
+ fprintf(stderr,"GPS_Track_New: Insufficient memory");
+ fflush(stderr);
+ return NULL;
+ }
+
+ return ret;
+}
+
+
+
+/* @func GPS_Track_Del ***********************************************
+**
+** Track destructor
+**
+** @param [w] thys [GPS_PTrack *] track to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Track_Del(GPS_PTrack *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+/* @func GPS_Way_New ***********************************************
+**
+** Waypoint constructor
+**
+** @return [GPS_PWay] virgin waypoint
+**********************************************************************/
+
+GPS_PWay GPS_Way_New(void)
+{
+ GPS_PWay ret;
+ int32 i;
+
+ if(!(ret=(GPS_PWay)malloc(sizeof(GPS_OWay))))
+ {
+ perror("malloc");
+ fprintf(stderr,"GPS_Way_New: Insufficient memory");
+ fflush(stderr);
+ return NULL;
+ }
+
+ /* Mark all as "unused" */
+ for(i=0;i<6;++i) ret->ident[i]=' ';
+ for(i=0;i<2;++i) ret->cc[i]=' ';
+ for(i=0;i<18;++i) ret->subclass[i]=' ';
+ for(i=0;i<40;++i) ret->cmnt[i]=' ';
+ for(i=0;i<24;++i) ret->city[i]=' ';
+ for(i=0;i<2;++i) ret->state[i]=' ';
+ for(i=0;i<30;++i) ret->name[i]=' ';
+ for(i=0;i<18;++i) ret->rte_link_subclass[i]=' ';
+ for(i=0;i<256;++i) ret->wpt_ident[i]=' ';
+ for(i=0;i<256;++i) ret->lnk_ident[i]=' ';
+ for(i=0;i<256;++i) ret->rte_link_ident[i]=' ';
+
+ ret->dst = ret->lat = ret->lon = GPS_FLTMAX;
+ ret->smbl = ret->dspl = ret->colour = ret->alt = ret->prot = INT_MAX;
+
+ if(gps_waypt_type==pD108)
+ {
+ ret->dst = 0;
+ ret->attr = 0x60;
+ }
+
+ return ret;
+}
+
+
+
+/* @func GPS_Way_Del ***********************************************
+**
+** Waypoint destructor
+**
+** @param [w] thys [GPS_Pway *] waypoint to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Way_Del(GPS_PWay *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Gsv_New ***********************************************
+**
+** Satellites in view constructor
+**
+** @return [GPS_PGsv] virgin siv
+**********************************************************************/
+
+GPS_PGsv GPS_Gsv_New(void)
+{
+ GPS_PGsv ret;
+
+ if(!(ret=(GPS_PGsv)malloc(sizeof(GPS_OGsv))))
+ return NULL;
+
+ ret->valid = ret->inview = 0;
+ *ret->elevation = *ret->azimuth = *ret->strength = '\0';
+
+ return ret;
+}
+
+
+
+/* @func GPS_Gsv_Del ***********************************************
+**
+** Satellites in view destructor
+**
+** @param [w] thys [GPS_PGsv *] siv to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Gsv_Del(GPS_PGsv *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Rme_New ***********************************************
+**
+** Position error constructor
+**
+** @return [GPS_PRme] virgin rme
+**********************************************************************/
+
+GPS_PRme GPS_Rme_New(void)
+{
+ GPS_PRme ret;
+
+ if(!(ret=(GPS_PRme)malloc(sizeof(GPS_ORme))))
+ return NULL;
+
+ ret->valid = 0;
+ ret->hpe = ret->vpe = ret->spe = (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Rme_Del ***********************************************
+**
+** Position error destructor
+**
+** @param [w] thys [GPS_PRme *] posn error to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Rme_Del(GPS_PRme *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Wpl_New ***********************************************
+**
+** Waypoint constructor
+**
+** @return [GPS_PWpl] virgin rme
+**********************************************************************/
+
+GPS_PWpl GPS_Wpl_New(void)
+{
+ GPS_PWpl ret;
+
+ if(!(ret=(GPS_PWpl)malloc(sizeof(GPS_OWpl))))
+ return NULL;
+
+ ret->valid = 0;
+ *ret->wpt = '\0';
+ ret->lat = ret->lon = (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Wpl_Del ***********************************************
+**
+** Waypoint destructor
+**
+** @param [w] thys [GPS_PWpl *] waypoint to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Wpl_Del(GPS_PWpl *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Gll_New ***********************************************
+**
+** Position (geographic lat/lon) constructor
+**
+** @return [GPS_PGll] virgin gll
+**********************************************************************/
+
+GPS_PGll GPS_Gll_New(void)
+{
+ GPS_PGll ret;
+
+ if(!(ret=(GPS_PGll)malloc(sizeof(GPS_OGll))))
+ return NULL;
+
+ ret->valid = 0;
+ ret->time = (time_t)0;
+ ret->lat = ret->lon = (double)0.;
+ ret->dv='\0';
+
+ return ret;
+}
+
+
+
+/* @func GPS_Gll_Del ***********************************************
+**
+** Position destructor
+**
+** @param [w] thys [GPS_PGll *] posn to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Gll_Del(GPS_PGll *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Rmz_New ***********************************************
+**
+** Altitude constructor
+**
+** @return [GPS_PRmz] virgin altitude
+**********************************************************************/
+
+GPS_PRmz GPS_Rmz_New(void)
+{
+ GPS_PRmz ret;
+
+ if(!(ret=(GPS_PRmz)malloc(sizeof(GPS_ORmz))))
+ return NULL;
+
+ ret->valid = ret->dim = ret->height = 0;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Rmz_Del ***********************************************
+**
+** Altitude destructor
+**
+** @param [w] thys [GPS_PRmz *] altitude to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Rmz_Del(GPS_PRmz *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Rmm_New ***********************************************
+**
+** Datum constructor
+**
+** @return [GPS_PRmm] virgin datum
+**********************************************************************/
+
+GPS_PRmm GPS_Rmm_New(void)
+{
+ GPS_PRmm ret;
+
+ if(!(ret=(GPS_PRmm)malloc(sizeof(GPS_ORmm))))
+ return NULL;
+
+ ret->valid = 0;
+ *ret->datum = '\0';
+
+ return ret;
+}
+
+
+
+/* @func GPS_Rmm_Del ***********************************************
+**
+** Datum destructor
+**
+** @param [w] thys [GPS_PRmm *] datum to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Rmm_Del(GPS_PRmm *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Bod_New ***********************************************
+**
+** Bearing constructor
+**
+** @return [GPS_PBod] virgin bearing
+**********************************************************************/
+
+GPS_PBod GPS_Bod_New(void)
+{
+ GPS_PBod ret;
+
+ if(!(ret=(GPS_PBod)malloc(sizeof(GPS_OBod))))
+ return NULL;
+
+ ret->valid = 0;
+ *ret->dest = *ret->start = '\0';
+ ret->true = ret->mag = (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Bod_Del ***********************************************
+**
+** Bearing destructor
+**
+** @param [w] thys [GPS_PBod *] bearing to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Bod_Del(GPS_PBod *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Rte_New ***********************************************
+**
+** Route (NMEA) constructor
+**
+** @return [GPS_PRte] virgin bearing
+**********************************************************************/
+
+GPS_PRte GPS_Rte_New(void)
+{
+ GPS_PRte ret;
+
+ if(!(ret=(GPS_PRte)malloc(sizeof(GPS_ORte))))
+ return NULL;
+
+ ret->valid = ret->rte = 0;
+ ret->type = '\0';
+ ret->wpts = NULL;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Rte_Del ***********************************************
+**
+** Route (NMEA) destructor
+**
+** @param [w] thys [GPS_PRte *] route to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Rte_Del(GPS_PRte *thys)
+{
+ if((*thys)->wpts)
+ free((void *)(*thys)->wpts);
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Rmc_New ***********************************************
+**
+** Minimum recommended specific constructor
+**
+** @return [GPS_PRmc] virgin minimum
+**********************************************************************/
+
+GPS_PRmc GPS_Rmc_New(void)
+{
+ GPS_PRmc ret;
+
+ if(!(ret=(GPS_PRmc)malloc(sizeof(GPS_ORmc))))
+ return NULL;
+
+ ret->valid = 0;
+ ret->time = (time_t)0;
+ *ret->date = ret->warn = '\0';
+ ret->lat = ret->lon = ret->speed = ret->cmg = ret->magvar =
+ (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Rmc_Del ***********************************************
+**
+** Minimum recommended specific destructor
+**
+** @param [w] thys [GPS_PRmc *] rec min to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Rmc_Del(GPS_PRmc *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Rmb_New ***********************************************
+**
+** Minimum recommended nav constructor
+**
+** @return [GPS_PRmb] virgin minimum nav
+**********************************************************************/
+
+GPS_PRmb GPS_Rmb_New(void)
+{
+ GPS_PRmb ret;
+
+ if(!(ret=(GPS_PRmb)malloc(sizeof(GPS_ORmb))))
+ return NULL;
+
+ ret->valid = 0;
+ *ret->owpt = *ret->dwpt = ret->warn = ret->correct = ret->alarm = '\0';
+ ret->cross = ret->lat = ret->lon = ret->range = ret->true = ret->velocity =
+ (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Rmb_Del ***********************************************
+**
+** Minimum recommended nav destructor
+**
+** @param [w] thys [GPS_PRmb *] rec min nav to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Rmb_Del(GPS_PRmb *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Gga_New ***********************************************
+**
+** Fix constructor
+**
+** @return [GPS_PGga] virgin fix
+**********************************************************************/
+
+GPS_PGga GPS_Gga_New(void)
+{
+ GPS_PGga ret;
+
+ if(!(ret=(GPS_PGga)malloc(sizeof(GPS_OGga))))
+ return NULL;
+
+ ret->time = (time_t)0.;
+ ret->valid = ret->qual = ret->nsat = ret->last = ret->dgpsid = 0;
+ ret->hdil = ret->lat = ret->lon = ret->alt = ret->galt = (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Gga_Del ***********************************************
+**
+** Fix destructor
+**
+** @param [w] thys [GPS_PGga *] fix to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Gga_Del(GPS_PGga *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Gsa_New ***********************************************
+**
+** DOP constructor
+**
+** @return [GPS_PGsa] virgin DOP
+**********************************************************************/
+
+GPS_PGsa GPS_Gsa_New(void)
+{
+ GPS_PGsa ret;
+
+ if(!(ret=(GPS_PGsa)malloc(sizeof(GPS_OGsa))))
+ return NULL;
+
+ ret->type = '\0';
+ ret->valid = ret->nsat = ret->fix = 0;
+ ret->pdop = ret->hdop = ret->vdop = (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Gsa_Del ***********************************************
+**
+** DOP destructor
+**
+** @param [w] thys [GPS_PGsa *] DOP to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Gsa_Del(GPS_PGsa *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Apb_New ***********************************************
+**
+** Autopilot B constructor
+**
+** @return [GPS_PApb] virgin autopilot
+**********************************************************************/
+
+GPS_PApb GPS_Apb_New(void)
+{
+ GPS_PApb ret;
+
+ if(!(ret=(GPS_PApb)malloc(sizeof(GPS_OApb))))
+ return NULL;
+
+ ret->blink = ret->warn = ret->steer = ret->unit = ret->alarmc =
+ ret->alarmp = *ret->wpt = '\0';
+ ret->valid = 0;
+ ret->edist = ret->od = ret->pd = ret->hdg = (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Apb_Del ***********************************************
+**
+** Autopilot destructor
+**
+** @param [w] thys [GPS_PApb *] autopilot to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Apb_Del(GPS_PApb *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Bwc_New ***********************************************
+**
+** Waypoint bng constructor
+**
+** @return [GPS_PBwc] virgin waypoint bng
+**********************************************************************/
+
+GPS_PBwc GPS_Bwc_New(void)
+{
+ GPS_PBwc ret;
+
+ if(!(ret=(GPS_PBwc)malloc(sizeof(GPS_OBwc))))
+ return NULL;
+
+ *ret->wpt = '\0';
+ ret->time = (time_t)0;
+ ret->valid = 0;
+ ret->lat = ret->lon = ret->true = ret->mag = ret->dist = (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Bwc_Del ***********************************************
+**
+** Waypoint bearing destructor
+**
+** @param [w] thys [GPS_PBwc *] waypoint bearing to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Bwc_Del(GPS_PBwc *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Bwr_New ***********************************************
+**
+** Waypoint bng rhumb constructor
+**
+** @return [GPS_PBwr] virgin waypoint bng
+**********************************************************************/
+
+GPS_PBwr GPS_Bwr_New(void)
+{
+ GPS_PBwr ret;
+
+ if(!(ret=(GPS_PBwr)malloc(sizeof(GPS_OBwr))))
+ return NULL;
+
+ *ret->wpt = '\0';
+ ret->time = (time_t)0;
+ ret->valid = 0;
+ ret->lat = ret->lon = ret->true = ret->mag = ret->dist = (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Bwr_Del ***********************************************
+**
+** Waypoint bearing rhumb destructor
+**
+** @param [w] thys [GPS_PBwr *] waypoint bearing rhumb to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Bwr_Del(GPS_PBwr *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Dbt_New ***********************************************
+**
+** Depth constructor
+**
+** @return [GPS_PDbt] virgin depth
+**********************************************************************/
+
+GPS_PDbt GPS_Dbt_New(void)
+{
+ GPS_PDbt ret;
+
+ if(!(ret=(GPS_PDbt)malloc(sizeof(GPS_ODbt))))
+ return NULL;
+
+ ret->valid = 0;
+ ret->f = ret->m = (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Dbt_Del ***********************************************
+**
+** Depth destructor
+**
+** @param [w] thys [GPS_PDbt *] depth to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Dbt_Del(GPS_PDbt *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Hdm_New ***********************************************
+**
+** Magnetic heading constructor
+**
+** @return [GPS_PHdm] virgin hdg
+**********************************************************************/
+
+GPS_PHdm GPS_Hdm_New(void)
+{
+ GPS_PHdm ret;
+
+ if(!(ret=(GPS_PHdm)malloc(sizeof(GPS_OHdm))))
+ return NULL;
+
+ ret->valid = 0;
+ ret->hdg = (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Hdm_Del ***********************************************
+**
+** Magnetic heading destructor
+**
+** @param [w] thys [GPS_PHdm *] mag hdg to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Hdm_Del(GPS_PHdm *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Hsc_New ***********************************************
+**
+** Heading to steer constructor
+**
+** @return [GPS_PHsc] virgin hdg
+**********************************************************************/
+
+GPS_PHsc GPS_Hsc_New(void)
+{
+ GPS_PHsc ret;
+
+ if(!(ret=(GPS_PHsc)malloc(sizeof(GPS_OHsc))))
+ return NULL;
+
+ ret->valid = 0;
+ ret->true = ret->mag = (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Hsc_Del ***********************************************
+**
+** Heading to steer destructor
+**
+** @param [w] thys [GPS_PHsc *] hdg to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Hsc_Del(GPS_PHsc *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Mtw_New ***********************************************
+**
+** Water temp constructor
+**
+** @return [GPS_PMtw] virgin temp
+**********************************************************************/
+
+GPS_PMtw GPS_Mtw_New(void)
+{
+ GPS_PMtw ret;
+
+ if(!(ret=(GPS_PMtw)malloc(sizeof(GPS_OMtw))))
+ return NULL;
+
+ ret->valid = 0;
+ ret->T = (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Mtw_Del ***********************************************
+**
+** Water temperature destructor
+**
+** @param [w] thys [GPS_PMtw *] water temp to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Mtw_Del(GPS_PMtw *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_R00_New ***********************************************
+**
+** Waypoint list constructor
+**
+** @return [GPS_PR00] virgin wpt list
+**********************************************************************/
+
+GPS_PR00 GPS_R00_New(void)
+{
+ GPS_PR00 ret;
+
+ if(!(ret=(GPS_PR00)malloc(sizeof(GPS_OR00))))
+ return NULL;
+
+ ret->valid = 0;
+ *ret->wpts='\0';
+
+ return ret;
+}
+
+
+
+/* @func GPS_R00_Del ***********************************************
+**
+** Waypoint list destructor
+**
+** @param [w] thys [GPS_PR00 *] waypoint list to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_R00_Del(GPS_PR00 *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Vhw_New ***********************************************
+**
+** Water speed constructor
+**
+** @return [GPS_PVhw] virgin water speed
+**********************************************************************/
+
+GPS_PVhw GPS_Vhw_New(void)
+{
+ GPS_PVhw ret;
+
+ if(!(ret=(GPS_PVhw)malloc(sizeof(GPS_OVhw))))
+ return NULL;
+
+ ret->valid = 0;
+ ret->true = ret->mag = ret->wspeed = ret->speed = (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Vhw_Del ***********************************************
+**
+** Water speed destructor
+**
+** @param [w] thys [GPS_PVhw *] waypoint list to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Vhw_Del(GPS_PVhw *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Vwr_New ***********************************************
+**
+** Wind constructor
+**
+** @return [GPS_PVwr] virgin wind
+**********************************************************************/
+
+GPS_PVwr GPS_Vwr_New(void)
+{
+ GPS_PVwr ret;
+
+ if(!(ret=(GPS_PVwr)malloc(sizeof(GPS_OVwr))))
+ return NULL;
+
+ ret->wdir = '\0';
+ ret->valid = 0;
+ ret->wind = ret->knots = ret->ms = ret->khr = (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Vwr_Del ***********************************************
+**
+** Wind destructor
+**
+** @param [w] thys [GPS_PVwr *] wind to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Vwr_Del(GPS_PVwr *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Vtg_New ***********************************************
+**
+** Track made good constructor
+**
+** @return [GPS_PVtg] virgin tmg
+**********************************************************************/
+
+GPS_PVtg GPS_Vtg_New(void)
+{
+ GPS_PVtg ret;
+
+ if(!(ret=(GPS_PVtg)malloc(sizeof(GPS_OVtg))))
+ return NULL;
+
+ ret->valid = 0;
+ ret->true = ret->mag = ret->knots = ret->khr = (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Vtg_Del ***********************************************
+**
+** Track made good destructor
+**
+** @param [w] thys [GPS_PVtg *] tmg to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Vtg_Del(GPS_PVtg *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Xte_New ***********************************************
+**
+** Cross track error constructor
+**
+** @return [GPS_Xte] virgin xte
+**********************************************************************/
+
+GPS_PXte GPS_Xte_New(void)
+{
+ GPS_PXte ret;
+
+ if(!(ret=(GPS_PXte)malloc(sizeof(GPS_OXte))))
+ return NULL;
+
+ ret->valid = 0;
+ ret->warn = ret->cycle = ret->steer = ret->unit = '\0';
+ ret->dist = (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Xte_Del ***********************************************
+**
+** Cross track error destructor
+**
+** @param [w] thys [GPS_PXte *] xte to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Xte_Del(GPS_PXte *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Xtr_New ***********************************************
+**
+** Cross track error dead constructor
+**
+** @return [GPS_Xtr] virgin xtr
+**********************************************************************/
+
+GPS_PXtr GPS_Xtr_New(void)
+{
+ GPS_PXtr ret;
+
+ if(!(ret=(GPS_PXtr)malloc(sizeof(GPS_OXtr))))
+ return NULL;
+
+ ret->valid = 0;
+ ret->steer = ret->unit = '\0';
+ ret->dist = (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Xtr_Del ***********************************************
+**
+** Cross track error dead destructor
+**
+** @param [w] thys [GPS_PXtr *] xtr to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Xtr_Del(GPS_PXtr *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Lib_New ***********************************************
+**
+** Link constructor
+**
+** @return [GPS_Lib] virgin link
+**********************************************************************/
+
+GPS_PLib GPS_Lib_New(void)
+{
+ GPS_PLib ret;
+
+ if(!(ret=(GPS_PLib)malloc(sizeof(GPS_OLib))))
+ return NULL;
+
+ ret->valid = 0;
+ ret->rqst = '\0';
+ ret->freq = ret->baud = (double)0.;
+
+ return ret;
+}
+
+
+
+/* @func GPS_Lib_Del ***********************************************
+**
+** Link destructor
+**
+** @param [w] thys [GPS_PLib *] link to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Lib_Del(GPS_PLib *thys)
+{
+ free((void *)*thys);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Nmea_New ***********************************************
+**
+** Nmea data constructor
+**
+** @return [GPS_PNmea] virgin nmea data
+**********************************************************************/
+
+GPS_PNmea GPS_Nmea_New(void)
+{
+ GPS_PNmea ret;
+
+ if(!(ret=(GPS_PNmea)malloc(sizeof(GPS_ONmea))))
+ return NULL;
+
+ ret->gsv = GPS_Gsv_New();
+ ret->rme = GPS_Rme_New();
+ ret->gll = GPS_Gll_New();
+ ret->rmz = GPS_Rmz_New();
+ ret->rmm = GPS_Rmm_New();
+ ret->bod = GPS_Bod_New();
+ ret->rte = GPS_Rte_New();
+ ret->wpl = GPS_Wpl_New();
+ ret->rmc = GPS_Rmc_New();
+ ret->rmb = GPS_Rmb_New();
+ ret->gga = GPS_Gga_New();
+ ret->gsa = GPS_Gsa_New();
+ ret->apb = GPS_Apb_New();
+ ret->bwc = GPS_Bwc_New();
+ ret->bwr = GPS_Bwr_New();
+ ret->dbt = GPS_Dbt_New();
+ ret->hdm = GPS_Hdm_New();
+ ret->hsc = GPS_Hsc_New();
+ ret->mtw = GPS_Mtw_New();
+ ret->r00 = GPS_R00_New();
+ ret->vhw = GPS_Vhw_New();
+ ret->vwr = GPS_Vwr_New();
+ ret->vtg = GPS_Vtg_New();
+ ret->xte = GPS_Xte_New();
+ ret->xtr = GPS_Xtr_New();
+ ret->lib = GPS_Lib_New();
+
+ return ret;
+}
+
+
+
+/* @func GPS_Nmea_Del ***********************************************
+**
+** NMEA data destructor
+**
+** @param [w] thys [GPS_PNmea *] nmea data to delete
+**
+** @return [void]
+**********************************************************************/
+
+void GPS_Nmea_Del(GPS_PNmea *thys)
+{
+
+ GPS_Gsv_Del(&(*thys)->gsv);
+ GPS_Rme_Del(&(*thys)->rme);
+ GPS_Gll_Del(&(*thys)->gll);
+ GPS_Rmz_Del(&(*thys)->rmz);
+ GPS_Rmm_Del(&(*thys)->rmm);
+ GPS_Bod_Del(&(*thys)->bod);
+ GPS_Rte_Del(&(*thys)->rte);
+ GPS_Wpl_Del(&(*thys)->wpl);
+ GPS_Rmc_Del(&(*thys)->rmc);
+ GPS_Rmb_Del(&(*thys)->rmb);
+ GPS_Gga_Del(&(*thys)->gga);
+ GPS_Gsa_Del(&(*thys)->gsa);
+ GPS_Apb_Del(&(*thys)->apb);
+ GPS_Bwc_Del(&(*thys)->bwc);
+ GPS_Bwr_Del(&(*thys)->bwr);
+ GPS_Dbt_Del(&(*thys)->dbt);
+ GPS_Hdm_Del(&(*thys)->hdm);
+ GPS_Hsc_Del(&(*thys)->hsc);
+ GPS_Mtw_Del(&(*thys)->mtw);
+ GPS_R00_Del(&(*thys)->r00);
+ GPS_Vhw_Del(&(*thys)->vhw);
+ GPS_Vwr_Del(&(*thys)->vwr);
+ GPS_Vtg_Del(&(*thys)->vtg);
+ GPS_Xte_Del(&(*thys)->xte);
+ GPS_Xtr_Del(&(*thys)->xtr);
+ GPS_Lib_Del(&(*thys)->lib);
+
+ free((void *)*thys);
+
+ return;
+}
--- /dev/null
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef gpsmem_h
+#define gpsmem_h
+
+
+#include "gps.h"
+
+GPS_PPacket GPS_Packet_New(void);
+void GPS_Packet_Del(GPS_PPacket *thys);
+GPS_PPvt_Data GPS_Pvt_New(void);
+void GPS_Pvt_Del(GPS_PPvt_Data *thys);
+GPS_PAlmanac GPS_Almanac_New(void);
+void GPS_Almanac_Del(GPS_PAlmanac *thys);
+GPS_PTrack GPS_Track_New(void);
+void GPS_Track_Del(GPS_PTrack *thys);
+GPS_PWay GPS_Way_New(void);
+void GPS_Way_Del(GPS_PWay *thys);
+
+
+/*
+ * NMEA Section
+ */
+GPS_PGsv GPS_Gsv_New(void);
+void GPS_Gsv_Del(GPS_PGsv *thys);
+GPS_PRme GPS_Rme_New(void);
+void GPS_Rme_Del(GPS_PRme *thys);
+GPS_PGll GPS_Gll_New(void);
+void GPS_Gll_Del(GPS_PGll *thys);
+GPS_PRmz GPS_Rmz_New(void);
+void GPS_Rmz_Del(GPS_PRmz *thys);
+GPS_PRmm GPS_Rmm_New(void);
+void GPS_Rmm_Del(GPS_PRmm *thys);
+GPS_PBod GPS_Bod_New(void);
+void GPS_Bod_Del(GPS_PBod *thys);
+GPS_PRte GPS_Rte_New(void);
+void GPS_Rte_Del(GPS_PRte *thys);
+GPS_PRmc GPS_Rmc_New(void);
+void GPS_Rmc_Del(GPS_PRmc *thys);
+GPS_PRmb GPS_Rmb_New(void);
+void GPS_Rmb_Del(GPS_PRmb *thys);
+GPS_PGga GPS_Gga_New(void);
+void GPS_Gga_Del(GPS_PGga *thys);
+GPS_PGsa GPS_Gsa_New(void);
+void GPS_Gsa_Del(GPS_PGsa *thys);
+GPS_PApb GPS_Apb_New(void);
+void GPS_Apb_Del(GPS_PApb *thys);
+GPS_PBwc GPS_Bwc_New(void);
+void GPS_Bwc_Del(GPS_PBwc *thys);
+GPS_PBwr GPS_Bwr_New(void);
+void GPS_Bwr_Del(GPS_PBwr *thys);
+GPS_PDbt GPS_Dbt_New(void);
+void GPS_Dbt_Del(GPS_PDbt *thys);
+GPS_PHdm GPS_Hdm_New(void);
+void GPS_Hdm_Del(GPS_PHdm *thys);
+GPS_PHsc GPS_Hsc_New(void);
+void GPS_Hsc_Del(GPS_PHsc *thys);
+GPS_PMtw GPS_Mtw_New(void);
+void GPS_Mtw_Del(GPS_PMtw *thys);
+GPS_PR00 GPS_R00_New(void);
+void GPS_R00_Del(GPS_PR00 *thys);
+GPS_PVhw GPS_Vhw_New(void);
+void GPS_Vhw_Del(GPS_PVhw *thys);
+GPS_PVwr GPS_Vwr_New(void);
+void GPS_Vwr_Del(GPS_PVwr *thys);
+GPS_PVtg GPS_Vtg_New(void);
+void GPS_Vtg_Del(GPS_PVtg *thys);
+GPS_PXte GPS_Xte_New(void);
+void GPS_Xte_Del(GPS_PXte *thys);
+GPS_PXtr GPS_Xtr_New(void);
+void GPS_Xtr_Del(GPS_PXtr *thys);
+GPS_PLib GPS_Lib_New(void);
+void GPS_Lib_Del(GPS_PLib *thys);
+GPS_PWpl GPS_Wpl_New(void);
+void GPS_Wpl_Del(GPS_PWpl *thys);
+GPS_PNmea GPS_Nmea_New(void);
+void GPS_Nmea_Del(GPS_PNmea *thys);
+
+
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef gpsnmea_h
+#define gpsnmea_h
+
+
+#include "gps.h"
+
+
+typedef struct GPS_SGsv
+{
+ int32 inview;
+ int32 prn[32];
+ int32 elevation[32];
+ int32 azimuth[32];
+ int32 strength[32];
+ int32 valid;
+} GPS_OGsv,*GPS_PGsv;
+
+typedef struct GPS_SRme
+{
+ double hpe;
+ double vpe;
+ double spe;
+ int32 valid;
+} GPS_ORme,*GPS_PRme;
+
+typedef struct GPS_SGll
+{
+ double lat;
+ double lon;
+ time_t time;
+ char dv;
+ int32 valid;
+} GPS_OGll,*GPS_PGll;
+
+typedef struct GPS_SRmz
+{
+ int32 height;
+ int32 dim;
+ int32 valid;
+} GPS_ORmz,*GPS_PRmz;
+
+typedef struct GPS_SRmm
+{
+ char datum[83];
+ int32 valid;
+} GPS_ORmm,*GPS_PRmm;
+
+typedef struct GPS_SBod
+{
+ double true;
+ double mag;
+ char dest[83];
+ char start[83];
+ int32 valid;
+} GPS_OBod,*GPS_PBod;
+
+typedef struct GPS_SRte
+{
+ char type;
+ int32 rte;
+ char *wpts;
+ int32 valid;
+} GPS_ORte,*GPS_PRte;
+
+typedef struct GPS_SWpl
+{
+ double lat;
+ double lon;
+ char wpt[83];
+ int32 valid;
+} GPS_OWpl,*GPS_PWpl;
+
+typedef struct GPS_SRmc
+{
+ time_t time;
+ char warn;
+ double lat;
+ double lon;
+ double speed;
+ double cmg;
+ char date[83];
+ double magvar;
+ int32 valid;
+} GPS_ORmc,*GPS_PRmc;
+
+typedef struct GPS_SRmb
+{
+ char warn;
+ double cross;
+ char correct;
+ char owpt[83];
+ char dwpt[83];
+ double lat;
+ double lon;
+ double range;
+ double true;
+ double velocity;
+ char alarm;
+ int32 valid;
+} GPS_ORmb,*GPS_PRmb;
+
+typedef struct GPS_SGga
+{
+ time_t time;
+ double lat;
+ double lon;
+ int32 qual;
+ int32 nsat;
+ double hdil;
+ double alt;
+ double galt;
+ int32 last;
+ int32 dgpsid;
+ int32 valid;
+} GPS_OGga,*GPS_PGga;
+
+typedef struct GPS_SGsa
+{
+ char type;
+ int32 fix;
+ int32 nsat;
+ int32 prn[12];
+ double pdop;
+ double hdop;
+ double vdop;
+ int32 valid;
+} GPS_OGsa,*GPS_PGsa;
+
+typedef struct GPS_SApb
+{
+ char blink;
+ char warn;
+ double edist;
+ char steer;
+ char unit;
+ char alarmc;
+ char alarmp;
+ double od;
+ char wpt[83];
+ double pd;
+ double hdg;
+ int32 valid;
+} GPS_OApb,*GPS_PApb;
+
+typedef struct GPS_SBwc
+{
+ time_t time;
+ double lat;
+ double lon;
+ double true;
+ double mag;
+ double dist;
+ char wpt[83];
+ int32 valid;
+} GPS_OBwc,*GPS_PBwc;
+
+typedef struct GPS_SBwr
+{
+ time_t time;
+ double lat;
+ double lon;
+ double true;
+ double mag;
+ double dist;
+ char wpt[83];
+ int32 valid;
+} GPS_OBwr,*GPS_PBwr;
+
+typedef struct GPS_SDbt
+{
+ double f;
+ double m;
+ int32 valid;
+} GPS_ODbt,*GPS_PDbt;
+
+typedef struct GPS_SHdm
+{
+ double hdg;
+ int32 valid;
+} GPS_OHdm,*GPS_PHdm;
+
+typedef struct GPS_SHsc
+{
+ double true;
+ double mag;
+ int32 valid;
+} GPS_OHsc,*GPS_PHsc;
+
+typedef struct GPS_SMtw
+{
+ double T;
+ int32 valid;
+} GPS_OMtw,*GPS_PMtw;
+
+typedef struct GPS_SR00
+{
+ char wpts[83];
+ int32 valid;
+} GPS_OR00,*GPS_PR00;
+
+typedef struct GPS_SVhw
+{
+ double true;
+ double mag;
+ double wspeed;
+ double speed;
+ int32 valid;
+} GPS_OVhw,*GPS_PVhw;
+
+typedef struct GPS_SVwr
+{
+ double wind;
+ char wdir;
+ double knots;
+ double ms;
+ double khr;
+ int32 valid;
+} GPS_OVwr,*GPS_PVwr;
+
+typedef struct GPS_SVtg
+{
+ double true;
+ double mag;
+ double knots;
+ double khr;
+ int32 valid;
+} GPS_OVtg,*GPS_PVtg;
+
+typedef struct GPS_SXte
+{
+ char warn;
+ char cycle;
+ double dist;
+ char steer;
+ char unit;
+ int32 valid;
+} GPS_OXte,*GPS_PXte;
+
+typedef struct GPS_SXtr
+{
+ double dist;
+ char steer;
+ char unit;
+ int32 valid;
+} GPS_OXtr,*GPS_PXtr;
+
+typedef struct GPS_SLib
+{
+ double freq;
+ double baud;
+ char rqst;
+ int32 valid;
+} GPS_OLib,*GPS_PLib;
+
+
+typedef struct GPS_SNmea
+{
+ GPS_PGsv gsv;
+ GPS_PRme rme;
+ GPS_PGll gll;
+ GPS_PRmz rmz;
+ GPS_PRmm rmm;
+ GPS_PBod bod;
+ GPS_PRte rte;
+ GPS_PWpl wpl;
+ GPS_PRmc rmc;
+ GPS_PRmb rmb;
+ GPS_PGga gga;
+ GPS_PGsa gsa;
+ GPS_PApb apb;
+ GPS_PBwc bwc;
+ GPS_PBwr bwr;
+ GPS_PDbt dbt;
+ GPS_PHdm hdm;
+ GPS_PHsc hsc;
+ GPS_PMtw mtw;
+ GPS_PR00 r00;
+ GPS_PVhw vhw;
+ GPS_PVwr vwr;
+ GPS_PVtg vtg;
+ GPS_PXte xte;
+ GPS_PXtr xtr;
+ GPS_PLib lib;
+} GPS_ONmea,*GPS_PNmea;
+
+
+
+
+
+
+extern int32 gps_fd; /* FD for serial port access [NMEA] */
+extern GPS_PNmea gps_nmea; /* Internal nmea data repository */
+
+
+
+void GPS_NMEA_Add_Checksum(char *s);
+int32 GPS_NMEA_Line_Check(const char *s);
+int32 GPS_NMEA_Load(int32 fd);
+int32 GPS_NMEA_Init(const char *s);
+void GPS_NMEA_Exit(void);
+int32 GPS_NMEA_Send(const char *s, int32 flag);
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef gpsnmeafmt_h
+#define gpsnmeafmt_h
+
+
+#include "gps.h"
+
+int32 GPS_NMEA_Apb_Scan(const char *s, GPS_PApb *thys);
+int32 GPS_NMEA_Bod_Scan(const char *s, GPS_PBod *thys);
+int32 GPS_NMEA_Bwc_Scan(const char *s, GPS_PBwc *thys);
+int32 GPS_NMEA_Bwr_Scan(const char *s, GPS_PBwr *thys);
+int32 GPS_NMEA_Dbt_Scan(const char *s, GPS_PDbt *thys);
+int32 GPS_NMEA_Gga_Scan(const char *s, GPS_PGga *thys);
+int32 GPS_NMEA_Gll_Scan(const char *s, GPS_PGll *thys);
+int32 GPS_NMEA_Gsa_Scan(const char *s, GPS_PGsa *thys);
+int32 GPS_NMEA_Gsv_Scan(const char *s, GPS_PGsv *thys);
+int32 GPS_NMEA_Hdm_Scan(const char *s, GPS_PHdm *thys);
+int32 GPS_NMEA_Hsc_Scan(const char *s, GPS_PHsc *thys);
+int32 GPS_NMEA_Mtw_Scan(const char *s, GPS_PMtw *thys);
+int32 GPS_NMEA_R00_Scan(const char *s, GPS_PR00 *thys);
+int32 GPS_NMEA_Rmb_Scan(const char *s, GPS_PRmb *thys);
+int32 GPS_NMEA_Rmc_Scan(const char *s, GPS_PRmc *thys);
+int32 GPS_NMEA_Rte_Scan(const char *s, GPS_PRte *thys);
+int32 GPS_NMEA_Vhw_Scan(const char *s, GPS_PVhw *thys);
+int32 GPS_NMEA_Vwr_Scan(const char *s, GPS_PVwr *thys);
+int32 GPS_NMEA_Vtg_Scan(const char *s, GPS_PVtg *thys);
+int32 GPS_NMEA_Wpl_Scan(const char *s, GPS_PWpl *thys);
+int32 GPS_NMEA_Xte_Scan(const char *s, GPS_PXte *thys);
+int32 GPS_NMEA_Xtr_Scan(const char *s, GPS_PXtr *thys);
+int32 GPS_NMEA_Rme_Scan(const char *s, GPS_PRme *thys);
+int32 GPS_NMEA_Rmz_Scan(const char *s, GPS_PRmz *thys);
+int32 GPS_NMEA_Rmm_Scan(const char *s, GPS_PRmm *thys);
+int32 GPS_NMEA_Lib_Scan(const char *s, GPS_PLib *thys);
+
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef gpsnmearead_h
+#define gpsnmearead_h
+
+
+#include "gps.h"
+
+int32 GPS_NMEA_Get_Apb(GPS_PApb *thys);
+int32 GPS_NMEA_Get_Bod(GPS_PBod *thys);
+int32 GPS_NMEA_Get_Bwc(GPS_PBwc *thys);
+int32 GPS_NMEA_Get_Bwr(GPS_PBwr *thys);
+int32 GPS_NMEA_Get_Dbt(GPS_PDbt *thys);
+int32 GPS_NMEA_Get_Gga(GPS_PGga *thys);
+int32 GPS_NMEA_Get_Gll(GPS_PGll *thys);
+int32 GPS_NMEA_Get_Gsa(GPS_PGsa *thys);
+int32 GPS_NMEA_Get_Gsv(GPS_PGsv *thys);
+int32 GPS_NMEA_Get_Hdm(GPS_PHdm *thys);
+int32 GPS_NMEA_Get_Hsc(GPS_PHsc *thys);
+int32 GPS_NMEA_Get_Mtw(GPS_PMtw *thys);
+int32 GPS_NMEA_Get_R00(GPS_PR00 *thys);
+int32 GPS_NMEA_Get_Rmb(GPS_PRmb *thys);
+int32 GPS_NMEA_Get_Rmc(GPS_PRmc *thys);
+int32 GPS_NMEA_Get_Rte(GPS_PRte *thys);
+int32 GPS_NMEA_Get_Vhw(GPS_PVhw *thys);
+int32 GPS_NMEA_Get_Vwr(GPS_PVwr *thys);
+int32 GPS_NMEA_Get_Vtg(GPS_PVtg *thys);
+int32 GPS_NMEA_Get_Wpl(GPS_PWpl *thys);
+int32 GPS_NMEA_Get_Xte(GPS_PXte *thys);
+int32 GPS_NMEA_Get_Xtr(GPS_PXtr *thys);
+int32 GPS_NMEA_Get_Rme(GPS_PRme *thys);
+int32 GPS_NMEA_Get_Rmz(GPS_PRmz *thys);
+int32 GPS_NMEA_Get_Rmm(GPS_PRmm *thys);
+int32 GPS_NMEA_Get_Lib(GPS_PLib *thys);
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null
+/*
+ * For portability any '32' type must be 32 bits
+ * and '16' type must be 16 bits
+ */
+typedef unsigned char UC;
+typedef short int16;
+typedef unsigned short uint16;
+typedef uint16 US;
+
+#if defined(__alpha)
+typedef int int32;
+typedef unsigned int uint32;
+#else
+typedef long int32;
+typedef unsigned long uint32;
+#endif
--- /dev/null
+/********************************************************************
+** @source JEEPS projection functions
+**
+** @author Copyright (C) 1999 Alan Bleasby
+** @version 1.0
+** @modified Feb 04 2000 Alan Bleasby. First version
+** @@
+**
+** This library is free software; you can redistribute it and/or
+** modify it under the terms of the GNU Library General Public
+** License as published by the Free Software Foundation; either
+** version 2 of the License, or (at your option) any later version.
+**
+** This library is distributed in the hope that it will be useful,
+** but WITHOUT ANY WARRANTY; without even the implied warranty of
+** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+** Library General Public License for more details.
+**
+** You should have received a copy of the GNU Library General Public
+** License along with this library; if not, write to the
+** Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+** Boston, MA 02111-1307, USA.
+********************************************************************/
+#include "gps.h"
+#include <math.h>
+#include <string.h>
+
+
+/* @func GPS_Math_Albers_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to Albers projection easting and
+** northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] phi1 [double] standard latitude (parallel) 1 (deg)
+** @param [r] phi2 [double] standard latitude (parallel) 2 (deg)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Albers_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi1, double phi2,
+ double phi0, double M0, double E0,
+ double N0, double a, double b)
+
+{
+ double dlambda;
+ double phis;
+ double phic;
+ double e;
+ double esq;
+ double esqs;
+ double omesqs2;
+
+ double a2;
+ double b2;
+ double q;
+ double q0;
+ double q1;
+ double q2;
+ double m1;
+ double m2;
+ double n;
+ double phi0s;
+ double phi1s;
+ double phi1c;
+ double phi2s;
+ double phi2c;
+ double ess;
+ double om0;
+ double m1sq;
+ double C;
+ double nq;
+ double nq0;
+ double rho;
+ double rho0;
+ double theta;
+
+ phi = GPS_Math_Deg_To_Rad(phi);
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ phi1 = GPS_Math_Deg_To_Rad(phi1);
+ phi2 = GPS_Math_Deg_To_Rad(phi2);
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ dlambda = lambda - M0;
+ if(dlambda > GPS_PI)
+ dlambda -= ((double)2.0 * GPS_PI);
+ if(dlambda < -GPS_PI)
+ dlambda += ((double)2.0 * GPS_PI);
+
+ phis = sin(phi);
+ phic = cos(phi);
+
+ a2 = a*a;
+ b2 = b*b;
+ esq = (a2-b2)/a2;
+ e = pow(esq,(double)0.5);
+
+
+ phi0s = sin(phi0);
+ ess = e * phi0s;
+ om0 = ((double)1.0 - ess*ess);
+ q0 = ((double)1.0 - esq) * (phi0s / om0-((double)1.0/(e+e)) *
+ log(((double)1.0-ess)/((double)1.0+ess)));
+ phi1s = sin(phi1);
+ phi1c = cos(phi1);
+ ess = e * phi1s;
+ om0 = ((double)1.0 - ess*ess);
+ m1 = phi1c/pow(om0,(double)0.5);
+ q1 = ((double)1.0 - esq) * (phi1s / om0-((double)1.0/(e+e)) *
+ log(((double)1.0-ess)/((double)1.0+ess)));
+
+ m1sq = m1*m1;
+ if(fabs(phi1-phi2)>1.0e-10)
+ {
+ phi2s = sin(phi2);
+ phi2c = cos(phi2);
+ ess = e * phi2s;
+ om0 = ((double)1.0 - ess*ess);
+ m2 = phi2c/pow(om0,(double)0.5);
+ q2 = ((double)1.0 - esq) * (phi2s / om0-((double)1.0/(e+e)) *
+ log(((double)1.0-ess)/((double)1.0+ess)));
+ n = (m1sq - m2*m2) / (q2-q1);
+ }
+ else
+ n = phi1s;
+
+ C = m1sq + n*q1;
+ nq0 = n * q0;
+ if(C < nq0)
+ rho0 = (double)0.;
+ else
+ rho0 = (a/n) * pow(C-nq0,(double)0.5);
+
+
+ esqs = e * phis;
+ omesqs2 = ((double)1.0 - esqs*esqs);
+ q = ((double)1.0 - esq) * (phis / omesqs2-((double)1.0/(e+e)) *
+ log(((double)1.0-esqs)/((double)1.0+esqs)));
+ nq = n*q;
+ if(C<nq)
+ rho = (double)0.;
+ else
+ rho = (a/n) * pow(C-nq,(double)0.5);
+
+ theta = n*dlambda;
+ *E = rho * sin(theta) + E0;
+ *N = rho0 - rho * cos(theta) + N0;
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Albers_EN_To_LatLon **********************************
+**
+** Convert Albers easting and northing to latitude and longitude
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] phi1 [double] standard latitude (parallel) 1 (deg)
+** @param [r] phi2 [double] standard latitude (parallel) 2 (deg)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Albers_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi1, double phi2,
+ double phi0, double M0, double E0,
+ double N0, double a, double b)
+{
+ double po2;
+ double rho;
+ double rho0;
+ double C;
+ double a2;
+ double b2;
+ double esq;
+ double e;
+ double phi0s;
+ double q0;
+ double q1;
+ double q2;
+ double phi1s;
+ double phi1c;
+ double phi2s;
+ double phi2c;
+ double m1;
+ double m1sq;
+ double m2;
+ double n;
+ double nq0;
+
+ double dx;
+ double dy;
+ double rhom;
+ double q;
+ double qc;
+ double qd2;
+ double rhon;
+ double lat;
+ double dphi;
+ double phis;
+ double ess;
+ double om0;
+ double theta;
+ double tol;
+
+
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ phi1 = GPS_Math_Deg_To_Rad(phi1);
+ phi2 = GPS_Math_Deg_To_Rad(phi2);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ a2 = a*a;
+ b2 = b*b;
+ esq = (a2-b2)/a2;
+ e = pow(esq,(double)0.5);
+
+
+ phi0s = sin(phi0);
+ ess = e * phi0s;
+ om0 = ((double)1.0 - ess*ess);
+ q0 = ((double)1.0 - esq) * (phi0s / om0-((double)1.0/(e+e)) *
+ log(((double)1.0-ess)/((double)1.0+ess)));
+ phi1s = sin(phi1);
+ phi1c = cos(phi1);
+ ess = e * phi1s;
+ om0 = ((double)1.0 - ess*ess);
+ m1 = phi1c/pow(om0,(double)0.5);
+ q1 = ((double)1.0 - esq) * (phi1s / om0-((double)1.0/(e+e)) *
+ log(((double)1.0-ess)/((double)1.0+ess)));
+
+ m1sq = m1*m1;
+ if(fabs(phi1-phi2)>1.0e-10)
+ {
+ phi2s = sin(phi2);
+ phi2c = cos(phi2);
+ ess = e * phi2s;
+ om0 = ((double)1.0 - ess*ess);
+ m2 = phi2c/pow(om0,(double)0.5);
+ q2 = ((double)1.0 - esq) * (phi2s / om0-((double)1.0/(e+e)) *
+ log(((double)1.0-ess)/((double)1.0+ess)));
+ n = (m1sq - m2*m2) / (q2-q1);
+ }
+ else
+ n = phi1s;
+
+ C = m1sq + n*q1;
+ nq0 = n * q0;
+ if(C < nq0)
+ rho0 = (double)0.;
+ else
+ rho0 = (a/n) * pow(C-nq0,(double)0.5);
+
+
+ dphi = (double) 1.0;
+ theta = (double) 0.0;
+ tol = (double) 4.85e-10;
+ po2 = (double)GPS_PI / (double)2.0;
+
+ dy = N-N0;
+ dx = E-E0;
+ rhom = rho0-dy;
+ rho = pow(dx*dx+rhom*rhom,(double)0.5);
+
+ if(n<0.0)
+ {
+ rho *= (double)-1.0;
+ dx *= (double)-1.0;
+ dy *= (double)-1.0;
+ rhom *= (double)-1.0;
+ }
+
+ if(rho)
+ theta = atan2(dx,rhom);
+ rhon = rho*n;
+ q = (C - (rhon*rhon) / a2) / n;
+ qc = (double)1.0 - ((double)1.0 / (e+e)) *
+ log(((double)1.0-e)/((double)1.0+e));
+ if(fabs(fabs(qc)-fabs(q))>1.9e-6)
+ {
+ qd2 = q/(double)2.0;
+ if(qd2>1.0)
+ *phi = po2;
+ else if(qd2<-1.0)
+ *phi = -po2;
+ else
+ {
+ lat = asin(qd2);
+ if(e<1.0e-10)
+ *phi = lat;
+ else
+ {
+ while(fabs(dphi)>tol)
+ {
+ phis = sin(lat);
+ ess = e*phis;
+ om0 = ((double)1.0 - ess*ess);
+ dphi = (om0*om0) / ((double)2.0*cos(lat))*
+ (q/((double)1.0-esq) - phis / om0 +
+ (log(((double)1.0-ess)/((double)1.0+ess)) /
+ (e+e)));
+ lat += dphi;
+ }
+ *phi = lat;
+ }
+
+ if(*phi > po2)
+ *phi = po2;
+ else if(*phi<-po2)
+ *phi = -po2;
+ }
+ }
+ else
+ {
+ if(q>=0.0)
+ *phi = po2;
+ else
+ *phi = -po2;
+ }
+
+ *lambda = M0 + theta / n;
+ if(*lambda > GPS_PI)
+ *lambda -= GPS_PI * (double)2.0;
+ if(*lambda < -GPS_PI)
+ *lambda += GPS_PI * (double)2.0;
+ if(*lambda>GPS_PI)
+ *lambda = GPS_PI;
+ else if(*lambda<-GPS_PI)
+ *lambda = -GPS_PI;
+
+ *phi = GPS_Math_Rad_To_Deg(*phi);
+ *lambda = GPS_Math_Rad_To_Deg(*lambda);
+
+ return;
+}
+
+
+
+/* @func GPS_Math_LambertCC_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to Lambert Conformal Conic projection
+** easting and northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] phi1 [double] standard latitude (parallel) 1 (deg)
+** @param [r] phi2 [double] standard latitude (parallel) 2 (deg)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_LambertCC_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi1, double phi2,
+ double phi0, double M0, double E0,
+ double N0, double a, double b)
+
+{
+ double po2;
+ double po4;
+ double a2;
+ double b2;
+ double phi0s;
+ double e;
+ double esq;
+ double ed2;
+ double ess;
+ double t0;
+ double t1;
+ double t2;
+ double m1;
+ double m2;
+ double phi1s;
+ double phi1c;
+ double phi2s;
+ double phi2c;
+ double n;
+ double F;
+ double Fa;
+ double rho;
+ double rho0;
+ double phis;
+ double t;
+ double theta;
+ double dphi;
+
+ phi = GPS_Math_Deg_To_Rad(phi);
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ phi1 = GPS_Math_Deg_To_Rad(phi1);
+ phi2 = GPS_Math_Deg_To_Rad(phi2);
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+
+ po2 = (double)GPS_PI / (double)2.0;
+ po4 = (double)GPS_PI / (double)4.0;
+ a2 = a*a;
+ b2 = b*b;
+ esq = (a2-b2)/a2;
+ e = pow(esq,(double)0.5);
+ ed2 = e / (double)2.0;
+
+ phi0s = sin(phi0);
+ ess = e * phi0s;
+ t0 = tan(po4-phi0/(double)2.0) / pow(((double)1.0-ess) /
+ ((double)1.0+ess),ed2);
+
+
+ phi1s = sin(phi1);
+ phi1c = cos(phi1);
+ ess = e * phi1s;
+ m1 = phi1c / pow(((double)1.0-ess*ess),(double)0.5);
+ t1 = tan(po4-phi1/(double)2.0) / pow(((double)1.0-ess) /
+ ((double)1.0+ess),ed2);
+
+ if(fabs(phi1-phi2)>1.0e-10)
+ {
+ phi2s = sin(phi2);
+ phi2c = cos(phi2);
+ ess = e * phi2s;
+ m2 = phi2c / pow(((double)1.0-ess*ess),(double)0.5);
+ t2 = tan(po4-phi2/(double)2.0) / pow(((double)1.0-ess) /
+ ((double)1.0+ess),ed2);
+ n = log(m1/m2) / log(t1/t2);
+ }
+ else
+ n = phi1s;
+
+ F = m1 / (n*pow(t1,n));
+ Fa = F*a;
+
+ rho0 = pow(t0,n) * Fa;
+
+ if(fabs(fabs(phi)-po2)>1.0e-10)
+ {
+ phis = sin(phi);
+ ess = e * phis;
+ t = tan(po4-phi/(double)2.0) / pow(((double)1.0-ess) /
+ ((double)1.0+ess),ed2);
+ rho = pow(t,n) * Fa;
+ }
+ else
+ {
+ if((phi*n)<=(double)0.0)
+ return;
+ rho = (double)0.0;
+ }
+
+ dphi = lambda - M0;
+ if(dphi>GPS_PI)
+ dphi -= (double)GPS_PI * (double)2.0;
+ if(dphi<-GPS_PI)
+ dphi += (double)GPS_PI * (double)2.0;
+ theta = dphi*n;
+
+ *E = rho * sin(theta) + E0;
+ *N = rho0 - rho * cos(theta) + N0;
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_LambertCC_EN_To_LatLon **********************************
+**
+** Convert Lambert Conformal Conic easting and northing to latitude and
+** longitude
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] phi1 [double] standard latitude (parallel) 1 (deg)
+** @param [r] phi2 [double] standard latitude (parallel) 2 (deg)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_LambertCC_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi1, double phi2,
+ double phi0, double M0, double E0,
+ double N0, double a, double b)
+{
+ double po2;
+ double po4;
+ double a2;
+ double b2;
+ double phi0s;
+ double e;
+ double esq;
+ double ed2;
+ double ess;
+ double t0;
+ double t1;
+ double t2;
+ double m1;
+ double m2;
+ double phi1s;
+ double phi1c;
+ double phi2s;
+ double phi2c;
+ double n;
+ double F;
+ double Fa;
+ double rho;
+ double rho0;
+ double phis;
+ double t;
+ double theta;
+
+ double dx;
+ double dy;
+ double rhom;
+ double lat;
+ double tlat;
+ double tol;
+
+
+
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ phi1 = GPS_Math_Deg_To_Rad(phi1);
+ phi2 = GPS_Math_Deg_To_Rad(phi2);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+
+ po2 = (double)GPS_PI / (double)2.0;
+ po4 = (double)GPS_PI / (double)4.0;
+ a2 = a*a;
+ b2 = b*b;
+ esq = (a2-b2)/a2;
+ e = pow(esq,(double)0.5);
+ ed2 = e / (double)2.0;
+
+ phi0s = sin(phi0);
+ ess = e * phi0s;
+ t0 = tan(po4-phi0/(double)2.0) / pow(((double)1.0-ess) /
+ ((double)1.0+ess),ed2);
+
+
+ phi1s = sin(phi1);
+ phi1c = cos(phi1);
+ ess = e * phi1s;
+ m1 = phi1c / pow(((double)1.0-ess*ess),(double)0.5);
+ t1 = tan(po4-phi1/(double)2.0) / pow(((double)1.0-ess) /
+ ((double)1.0+ess),ed2);
+
+ if(fabs(phi1-phi2)>1.0e-10)
+ {
+ phi2s = sin(phi2);
+ phi2c = cos(phi2);
+ ess = e * phi2s;
+ m2 = phi2c / pow(((double)1.0-ess*ess),(double)0.5);
+ t2 = tan(po4-phi2/(double)2.0) / pow(((double)1.0-ess) /
+ ((double)1.0+ess),ed2);
+ n = log(m1/m2) / log(t1/t2);
+ }
+ else
+ n = phi1s;
+
+ F = m1 / (n*pow(t1,n));
+ Fa = F*a;
+
+ rho0 = pow(t0,n) * Fa;
+
+ tlat = theta = (double)0.0;
+ tol = (double)4.85e-10;
+
+ dx = E - E0;
+ dy = N - N0;
+ rhom = rho0 - dy;
+ rho = pow(dx*dx + rhom*rhom,(double)0.5);
+
+ if(n<0.0)
+ {
+ rhom *= (double)-1.0;
+ dy *= (double)-1.0;
+ dx *= (double)-1.0;
+ rho *= (double)-1.0;
+ }
+
+ if(rho)
+ {
+ theta = atan2(dx,rhom);
+ t = pow(rho/Fa,(double)1.0/n);
+ lat = po2 - (double)2.0*atan(t);
+ while(fabs(lat-tlat)>tol)
+ {
+ tlat = lat;
+ phis = sin(lat);
+ ess = e * phis;
+ lat = po2 - (double)2.0 * atan(t*pow(((double)1.0-ess) /
+ ((double)1.0+ess),
+ e / (double)2.0));
+ }
+ *phi = lat;
+ *lambda = theta/n + M0;
+
+ if(*phi>po2)
+ *phi=po2;
+ else if(*phi<-po2)
+ *phi=-po2;
+ if(*lambda>GPS_PI)
+ *lambda -= (double)GPS_PI * (double)2.0;
+ if(*lambda<-GPS_PI)
+ *lambda += (double)GPS_PI * (double)2.0;
+
+ if(*lambda>GPS_PI)
+ *lambda = GPS_PI;
+ else if(*lambda<-GPS_PI)
+ *lambda = -GPS_PI;
+ }
+ else
+ {
+ if(n>0.0)
+ *phi = po2;
+ else
+ *phi = -po2;
+ *lambda = M0;
+ }
+
+ *lambda = GPS_Math_Rad_To_Deg(*lambda);
+ *phi = GPS_Math_Rad_To_Deg(*phi);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Miller_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to Miller Cylindrical projection easting and
+** northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Miller_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double M0, double E0,
+ double N0, double a, double b)
+{
+ double a2;
+ double b2;
+ double R;
+ double e2;
+ double e4;
+ double e6;
+ double p2;
+ double po2;
+ double phis;
+ double dlam;
+
+
+ phi = GPS_Math_Deg_To_Rad(phi);
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ po2 = (double)GPS_PI / (double)2.0;
+ p2 = (double)GPS_PI * (double)2.0;
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e4*e2;
+
+ R = a*((double)1.0-e2/(double)6.0-(double)17.0*e4/(double)360.0-
+ (double)67.0*e6/(double)3024.0);
+
+ if(M0>GPS_PI)
+ M0 -= p2;
+
+ phis = sin((double)0.8 * phi);
+
+ dlam = lambda - M0;
+ if(dlam>GPS_PI)
+ dlam-=p2;
+ if(dlam<-GPS_PI)
+ dlam+=p2;
+
+ *E = R*dlam+E0;
+ *N = (R/(double)1.6) * log(((double)1.0+phis) / ((double)1.0-phis)) + N0;
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Miller_EN_To_LatLon **********************************
+**
+** Convert latitude and longitude to Miller Cylindrical projection easting and
+** northing
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Miller_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double M0, double E0,
+ double N0, double a, double b)
+{
+ double a2;
+ double b2;
+ double R;
+ double e;
+ double e2;
+ double e4;
+ double e6;
+ double p2;
+ double po2;
+ double dx;
+ double dy;
+
+ dx = E - E0;
+ dy = N - N0;
+
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ po2 = (double)GPS_PI / (double)2.0;
+ p2 = (double)GPS_PI * (double)2.0;
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e4*e2;
+ e = pow(e2,(double)0.5);
+
+ R = a*((double)1.0-e2/(double)6.0-(double)17.0*e4/(double)360.0-
+ (double)67.0*e6/(double)3024.0);
+ if(M0>GPS_PI)
+ M0 -= p2;
+
+ *phi = atan(sinh((double)0.8*dy/R)) / (double)0.8;
+ *lambda = M0+dx/R;
+
+ if(*phi>po2)
+ *phi=po2;
+ else if (*phi<-po2)
+ *phi=-po2;
+
+ if(*lambda>GPS_PI)
+ *lambda-=p2;
+ if(*lambda<-GPS_PI)
+ *lambda+=p2;
+
+ if(*lambda>GPS_PI)
+ *lambda=GPS_PI;
+ else if(*lambda<-GPS_PI)
+ *lambda=-GPS_PI;
+
+ *lambda = GPS_Math_Rad_To_Deg(*lambda);
+ *phi = GPS_Math_Rad_To_Deg(*phi);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Bonne_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to Bonne pseudoconic equal area projection
+** easting and northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Bonne_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi0, double M0, double E0,
+ double N0, double a, double b)
+{
+ double p2;
+ double po2;
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double M1;
+ double m1;
+ double c0;
+ double c1;
+ double c2;
+ double c3;
+ double j;
+ double te4;
+ double E1;
+ double E2;
+ double E3;
+ double E4;
+ double x;
+ double phi0s;
+ double lat;
+ double phi0c;
+ double phi0s2;
+ double phi0s4;
+ double phi0s6;
+ double as;
+
+ double phis;
+ double phic;
+ double phis2;
+ double phis4;
+ double phis6;
+ double dlam;
+ double mm;
+ double MM;
+ double rho;
+ double EE;
+ double tol;
+
+
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+ phi = GPS_Math_Deg_To_Rad(phi);
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ phi0s = sin(phi0);
+ p2 = (double)GPS_PI * (double)2.0;
+ po2 = (double)GPS_PI / (double)2.0;
+ if(M0>GPS_PI)
+ M0 -= p2;
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+
+ j = (double)45.0*e6/(double)1024.0;
+ te4 = (double)3.0 * e4;
+ c0 = (double)1.0-e2/(double)4.0-te4/(double)64.0-(double)5.0*e6/
+ (double)256.0;
+ c1 = (double)3.0*e2/(double)8.0+te4/(double)32.0+j;
+ c2 = (double)15.0*e4/(double)256.0+j;
+ c3 = (double)35.0*e6/(double)3072.0;
+
+ phi0c = cos(phi0);
+ m1 = phi0c/ pow(((double)1.0-e2*phi0s*phi0s),(double)0.5);
+ lat = c0 * phi0;
+
+ phi0s2 = c1 * sin((double)2.0*phi0);
+ phi0s4 = c2 * sin((double)4.0*phi0);
+ phi0s6 = c3 * sin((double)6.0*phi0);
+ M1 = a*(lat-phi0s2+phi0s4-phi0s6);
+
+ x = pow((double)1.0-e2,(double)0.5);
+ E1 = ((double)1.0-x) / ((double)1.0+x);
+ E2 = E1*E1;
+ E3 = E2*E1;
+ E4 = E3*E1;
+
+ if(!phi0s)
+ as = (double)0.0;
+ else
+ as = a*m1/phi0s;
+
+
+ dlam = lambda - M0;
+ if(dlam>GPS_PI)
+ dlam -= p2;
+ if(dlam<-GPS_PI)
+ dlam += p2;
+
+ phis = sin(phi);
+ phic = cos(phi);
+
+ tol = (double)0.0001;
+ if(!(phi-phi0) && (((po2-tol)<fabs(phi)) && (fabs(phi)<po2+tol)))
+ *E = *N = (double)0.0;
+ else
+ {
+ mm = phic / pow(((double)1.0-e2*phis*phis),(double)0.5);
+ lat = c0 * phi;
+ phis2 = c1 * sin((double)2.0*phi);
+ phis4 = c2 * sin((double)4.0*phi);
+ phis6 = c3 * sin((double)6.0*phi);
+ MM = a * (lat-phis2+phis4-phis6);
+
+ rho = as + M1 - MM;
+ if(!rho)
+ EE = (double)0.0;
+ else
+ EE = a * mm * dlam / rho;
+
+ *E = rho * sin(EE) + E0;
+ *N = as - rho * cos(EE) + N0;
+ }
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Bonne_EN_To_LatLon **********************************
+**
+** Convert Bonne pseudoconic equal area easting and northing projection
+** to latitude and longitude
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Bonne_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0, double M0,
+ double E0, double N0, double a, double b)
+{
+ double p2;
+ double po2;
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double M1;
+ double m1;
+ double c0;
+ double c1;
+ double c2;
+ double c3;
+ double A0;
+ double A1;
+ double A2;
+ double A3;
+ double j;
+ double te4;
+ double E1;
+ double E2;
+ double E3;
+ double E4;
+ double x;
+ double phi0s;
+ double lat;
+ double phi0c;
+ double phi0s2;
+ double phi0s4;
+ double phi0s6;
+ double as;
+
+ double phis;
+ double phic;
+ double dx;
+ double dy;
+ double mu;
+ double mm;
+ double MM;
+ double asdy;
+ double rho;
+ double smu2;
+ double smu4;
+ double smu6;
+ double smu8;
+ double tol;
+
+
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ phi0s = sin(phi0);
+ p2 = (double)GPS_PI * (double)2.0;
+ po2 = (double)GPS_PI / (double)2.0;
+ if(M0>GPS_PI)
+ M0 -= p2;
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+
+ j = (double)45.0*e6/(double)1024.0;
+ te4 = (double)3.0 * e4;
+ c0 = (double)1.0-e2/(double)4.0-te4/(double)64.0-(double)5.0*e6/
+ (double)256.0;
+ c1 = (double)3.0*e2/(double)8.0+te4/(double)32.0+j;
+ c2 = (double)15.0*e4/(double)256.0+j;
+ c3 = (double)35.0*e6/(double)3072.0;
+
+ phi0c = cos(phi0);
+ m1 = phi0c/ pow(((double)1.0-e2*phi0s*phi0s),(double)0.5);
+ lat = c0 * phi0;
+
+ phi0s2 = c1 * sin((double)2.0*phi0);
+ phi0s4 = c2 * sin((double)4.0*phi0);
+ phi0s6 = c3 * sin((double)6.0*phi0);
+ M1 = a*(lat-phi0s2+phi0s4-phi0s6);
+
+ x = pow((double)1.0-e2,(double)0.5);
+ E1 = ((double)1.0-x) / ((double)1.0+x);
+ E2 = E1*E1;
+ E3 = E2*E1;
+ E4 = E3*E1;
+ A0 = (double)3.0*E1/(double)2.0-(double)27.0*E3/(double)32.0;
+ A1 = (double)21.0*E2/(double)16.0-(double)55.0*E4/(double)32.0;
+ A2 = (double)151.0*E3/(double)96.0;
+ A3 = (double)1097.0*E4/(double)512.0;
+ if(!phi0s)
+ as = (double)0.0;
+ else
+ as = a*m1/phi0s;
+
+
+ dx = E - E0;
+ dy = N - N0;
+ asdy = as - dy;
+ rho = pow(dx*dx+asdy*asdy,(double)0.5);
+ if(phi0<(double)0.0)
+ rho=-rho;
+ MM = as+M1-rho;
+
+ mu = MM / (a*c0);
+ smu2 = A0 * sin((double)2.0*mu);
+ smu4 = A1 * sin((double)4.0*mu);
+ smu6 = A2 * sin((double)6.0*mu);
+ smu8 = A3 * sin((double)8.0*mu);
+ *phi = mu+smu2+smu4+smu6+smu8;
+
+ tol = (double)0.00001;
+ if(((po2-tol)<fabs(*phi)) && (fabs(*phi)<po2+tol))
+ *lambda = M0;
+ else
+ {
+ phic = cos(*phi);
+ phis = sin(*phi);
+ mm = phic / pow(((double)1.0-e2*phis*phis),(double)0.5);
+ if(phi0<(double)0.0)
+ {
+ dx = -dx;
+ asdy = -asdy;
+ }
+ *lambda = M0 + rho * (atan2(dx,asdy)) / (a * mm);
+ }
+
+ if(*phi>po2)
+ *phi = po2;
+ else if(*phi<-po2)
+ *phi = -po2;
+
+ if(*lambda>GPS_PI)
+ *lambda -= p2;
+ if(*lambda<-GPS_PI)
+ *lambda += p2;
+
+ if(*lambda>GPS_PI)
+ *lambda = GPS_PI;
+ else if(*lambda<-GPS_PI)
+ *lambda=-GPS_PI;
+
+ *lambda = GPS_Math_Rad_To_Deg(*lambda);
+ *phi = GPS_Math_Rad_To_Deg(*phi);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Cassini_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to Cassini transverse cylindrical projection
+** easting and northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Cassini_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi0, double M0,
+ double E0, double N0, double a, double b)
+{
+ double p2;
+ double po2;
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double AM0;
+ double c0;
+ double c1;
+ double c2;
+ double c3;
+ double om0;
+ double A0;
+ double A1;
+ double A2;
+ double A3;
+ double j;
+ double te4;
+ double phi0s2;
+ double phi0s4;
+ double phi0s6;
+ double lat;
+ double x;
+ double E1;
+ double E2;
+ double E3;
+ double E4;
+
+ double phis;
+ double phic;
+ double phit;
+ double phis2;
+ double phis4;
+ double phis6;
+ double RD;
+ double dlam;
+ double NN;
+ double TT;
+ double WW;
+ double WW2;
+ double WW3;
+ double WW4;
+ double WW5;
+ double CC;
+ double MM;
+
+
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ phi = GPS_Math_Deg_To_Rad(phi);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+
+ p2 = (double)GPS_PI * (double)2.;
+ po2 = (double)GPS_PI / (double)2.;
+
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+
+ te4 = (double)3.0 * e4;
+ j = (double)45. * e6 / (double)1024.;
+ c0 = (double)1.0-e2/(double)4.-te4/(double)64.-(double)5.*e6/(double)256.;
+ c1 = (double)3.*e2/(double)8.+te4/(double)32.+j;
+ c2 = (double)15.*e4/(double)256.+j;
+ c3 = (double)35.*e6/(double)3072.;
+
+ lat = c0*phi0;
+ phi0s2 = c1 * sin((double)2.*phi0);
+ phi0s4 = c2 * sin((double)4.*phi0);
+ phi0s6 = c3 * sin((double)6.*phi0);
+ AM0 = a * (lat-phi0s2+phi0s4-phi0s6);
+
+ om0 = (double)1.0 - e2;
+ x = pow(om0,(double)0.5);
+ E1 = ((double)1.0 - x) / ((double)1.0 + x);
+ E2 = E1*E1;
+ E3 = E1*E2;
+ E4 = E1*E3;
+ A0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.;
+ A1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.;
+ A2 = (double)151.*E3/(double)96.;
+ A3 = (double)1097.*E4/(double)512.;
+
+
+ dlam = lambda - M0;
+ if(dlam>GPS_PI)
+ dlam -= p2;
+ if(dlam<-GPS_PI)
+ dlam += p2;
+
+ phis = sin(phi);
+ phic = cos(phi);
+ phit = tan(phi);
+ RD = pow((double)1.-e2*phis*phis,(double).5);
+ NN = a/RD;
+ TT = phit*phit;
+ WW = dlam*phic;
+ WW2 = WW*WW;
+ WW3 = WW*WW2;
+ WW4 = WW*WW3;
+ WW5 = WW*WW4;
+ CC = e2*phic*phic/om0;
+ lat = c0*phi;
+ phis2 = c1 * sin((double)2.*phi);
+ phis4 = c2 * sin((double)4.*phi);
+ phis6 = c3 * sin((double)6.*phi);
+ MM = a * (lat-phis2+phis4-phis6);
+
+ *E = NN*(WW-(TT*WW3/(double)6.)-((double)8.-TT+(double)8.*CC)*
+ (TT*WW5/(double)120.)) + E0;
+ *N = MM-AM0+NN*phit*((WW2/(double)2.)+((double)5.-TT+(double)6.*CC) *
+ WW4/(double)24.) + N0;
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Cassini_EN_To_LatLon **********************************
+**
+** Convert Cassini transverse cylindrical easting and northing projection
+** to latitude and longitude
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Cassini_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0, double M0,
+ double E0, double N0, double a, double b)
+
+{
+ double p2;
+ double po2;
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double AM0;
+ double c0;
+ double c1;
+ double c2;
+ double c3;
+ double om0;
+ double A0;
+ double A1;
+ double A2;
+ double A3;
+ double j;
+ double te4;
+ double phi0s2;
+ double phi0s4;
+ double phi0s6;
+ double lat;
+ double x;
+ double E1;
+ double E2;
+ double E3;
+ double E4;
+
+ double dx;
+ double dy;
+ double mu;
+ double mus2;
+ double mus4;
+ double mus6;
+ double mus8;
+ double M1;
+ double phi1;
+ double phi1s;
+ double phi1c;
+ double phi1t;
+ double T;
+ double T1;
+ double N1;
+ double R1;
+ double RD;
+ double DD;
+ double D2;
+ double D3;
+ double D4;
+ double D5;
+ double tol;
+
+ M0 = GPS_Math_Deg_To_Rad(M0);
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+
+ p2 = (double)GPS_PI * (double)2.;
+ po2 = (double)GPS_PI / (double)2.;
+
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+
+ te4 = (double)3.0 * e4;
+ j = (double)45. * e6 / (double)1024.;
+ c0 = (double)1.0-e2/(double)4.-te4/(double)64.-(double)5.*e6/(double)256.;
+ c1 = (double)3.*e2/(double)8.+te4/(double)32.+j;
+ c2 = (double)15.*e4/(double)256.+j;
+ c3 = (double)35.*e6/(double)3072.;
+
+ lat = c0*phi0;
+ phi0s2 = c1 * sin((double)2.*phi0);
+ phi0s4 = c2 * sin((double)4.*phi0);
+ phi0s6 = c3 * sin((double)6.*phi0);
+ AM0 = a * (lat-phi0s2+phi0s4-phi0s6);
+
+ om0 = (double)1.0 - e2;
+ x = pow(om0,(double)0.5);
+ E1 = ((double)1.0 - x) / ((double)1.0 + x);
+ E2 = E1*E1;
+ E3 = E1*E2;
+ E4 = E1*E3;
+ A0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.;
+ A1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.;
+ A2 = (double)151.*E3/(double)96.;
+ A3 = (double)1097.*E4/(double)512.;
+
+
+
+ tol = (double)1.e-5;
+
+ dx = E - E0;
+ dy = N - N0;
+ M1 = AM0 + dy;
+ mu = M1 / (a*c0);
+ mus2 = A0 * sin((double)2.*mu);
+ mus4 = A1 * sin((double)4.*mu);
+ mus6 = A2 * sin((double)6.*mu);
+ mus8 = A3 * sin((double)8.*mu);
+ phi1 = mu + mus2 + mus4 + mus6 + mus8;
+
+ if((((po2-tol)<phi1)&&(phi1<(po2+tol))))
+ {
+ *phi = po2;
+ *lambda = M0;
+ }
+ else if((((-po2-tol)<phi1)&&(phi1<(-po2+tol))))
+ {
+ *phi = -po2;
+ *lambda = M0;
+ }
+ else
+ {
+ phi1s = sin(phi1);
+ phi1c = cos(phi1);
+ phi1t = tan(phi1);
+ T1 = phi1t*phi1t;
+ RD = pow((double)1.-e2*phi1s*phi1s,(double).5);
+ N1 = a/RD;
+ R1 = N1 * om0 / (RD*RD);
+ DD = dx/N1;
+ D2 = DD*DD;
+ D3 = DD*D2;
+ D4 = DD*D3;
+ D5 = DD*D4;
+ T = (double)1. + (double)3.*T1;
+ *phi = phi1-(N1*phi1t/R1)*(D2/(double)2.-T*D4/(double)24.);
+ *lambda = M0+(DD-T1*D3/(double)3.+T*T1*D5/(double)15.)/phi1c;
+
+ if(*phi>po2)
+ *phi=po2;
+ else if(*phi<-po2)
+ *phi=-po2;
+
+ if(*lambda>GPS_PI)
+ *lambda -= p2;
+ if(*lambda<-GPS_PI)
+ *lambda += p2;
+
+ if(*lambda>GPS_PI)
+ *lambda=GPS_PI;
+ else if(*lambda<-GPS_PI)
+ *lambda=-GPS_PI;
+ }
+
+ *lambda = GPS_Math_Rad_To_Deg(*lambda);
+ *phi = GPS_Math_Rad_To_Deg(*phi);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Cylea_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to Cylindrical equal area projection
+** easting and northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Cylea_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi0, double M0,
+ double E0, double N0, double a, double b)
+{
+ double a2;
+ double b2;
+ double e;
+ double e2;
+ double e4;
+ double e6;
+ double k0;
+ double ak0;
+ double k2;
+ double c0;
+ double c1;
+ double c2;
+ double p2;
+ double po2;
+ double phi0s;
+ double phi0c;
+
+ double dlam;
+ double qq;
+ double x;
+ double phis;
+
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ phi = GPS_Math_Deg_To_Rad(phi);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ p2 = (double)GPS_PI * (double)2.;
+ po2 = (double)GPS_PI / (double)2.;
+
+ if(M0>GPS_PI)
+ M0-=p2;
+
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+ e = pow(e2,(double).5);
+ c0 = e2/(double)3.+(double)31.*e4/(double)180.+(double)517.*
+ e6/(double)5040.;
+ c1 = (double)23.*e4/(double)360.+(double)251.*e6/(double)3780.;
+ c2 = (double)761.*e6/(double)45360.;
+
+ phi0s = sin(phi0);
+ phi0c = cos(phi0);
+ k0 = phi0c / pow((double)1.-e2*phi0s*phi0s,(double).5);
+ ak0 = a*k0;
+ k2 = k0 * (double)2.;
+
+ dlam = lambda - M0;
+ if(dlam>GPS_PI)
+ dlam-=p2;
+ if(dlam<-GPS_PI)
+ dlam+=p2;
+
+ phis = sin(phi);
+ x = e * phis;
+ qq = ((double)1.-e2)*(phis/((double)1.-x*x)-((double)1./((double)2.*e))*
+ log(((double)1.-x)/((double)1.+x)));
+ *E = ak0 * dlam + E0;
+ *N = a * qq / k2 + N0;
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Cylea_EN_To_LatLon **********************************
+**
+** Convert Cylindrical equal area easting and northing projection
+** to latitude and longitude
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Cylea_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0, double M0,
+ double E0, double N0, double a, double b)
+
+{
+ double a2;
+ double b2;
+ double e;
+ double e2;
+ double e4;
+ double e6;
+ double k0;
+ double ak0;
+ double k2;
+ double c0;
+ double c1;
+ double c2;
+ double p2;
+ double po2;
+ double phi0s;
+ double phi0c;
+
+ double dx;
+ double dy;
+ double qp;
+ double bt;
+ double phis;
+ double i;
+ double x;
+ double bs2;
+ double bs4;
+ double bs6;
+
+
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ p2 = (double)GPS_PI * (double)2.;
+ po2 = (double)GPS_PI / (double)2.;
+
+ if(M0>GPS_PI)
+ M0-=p2;
+
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+ e = pow(e2,(double).5);
+ c0 = e2/(double)3.+(double)31.*e4/(double)180.+(double)517.*
+ e6/(double)5040.;
+ c1 = (double)23.*e4/(double)360.+(double)251.*e6/(double)3780.;
+ c2 = (double)761.*e6/(double)45360.;
+
+ phi0s = sin(phi0);
+ phi0c = cos(phi0);
+ k0 = phi0c / pow((double)1.-e2*phi0s*phi0s,(double).5);
+ ak0 = a*k0;
+ k2 = k0 * (double)2.;
+
+ dx = E - E0;
+ dy = N - N0;
+ phis = sin(po2);
+ x = e*phis;
+ qp = ((double)1.-e2)*(phis/((double)1.-x*x)-((double)1./((double)2.*e))*
+ log(((double)1.-x)/((double)1.+x)));
+ i = k2*dy/(a*qp);
+ if(i>(double)1.)
+ i=(double)1.;
+ else if(i<(double)-1.)
+ i=(double)-1.;
+ bt = asin(i);
+ bs2 = c0 * sin((double)2.*bt);
+ bs4 = c1 * sin((double)4.*bt);
+ bs6 = c2 * sin((double)6.*bt);
+
+ *phi = bt+bs2+bs4+bs6;
+ *lambda = M0 + dx/ak0;
+
+ if(*phi>po2)
+ *phi=po2;
+ else if(*phi<-po2)
+ *phi=-po2;
+
+ if(*lambda>GPS_PI)
+ *lambda -= p2;
+ if(*lambda<-GPS_PI)
+ *lambda += p2;
+
+ if(*lambda>GPS_PI)
+ *lambda=GPS_PI;
+ else if(*lambda<-GPS_PI)
+ *lambda=-GPS_PI;
+
+ *lambda = GPS_Math_Rad_To_Deg(*lambda);
+ *phi = GPS_Math_Rad_To_Deg(*phi);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_EckertIV_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to Eckert IV equal area elliptical
+** pseudocylindrical projection easting and northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_EckertIV_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double M0, double E0, double N0,
+ double a, double b)
+{
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double Ra0;
+ double Ra1;
+ double po2;
+ double p2;
+
+ double Ra;
+
+ double phis;
+ double theta;
+ double dtheta;
+ double thetas;
+ double thetac;
+ double n;
+ double dlam;
+ double tol;
+
+
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+ phi = GPS_Math_Deg_To_Rad(phi);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ p2 = (double)GPS_PI * (double)2.;
+ po2 = (double)GPS_PI / (double)2.;
+
+ if(M0>GPS_PI)
+ M0-=p2;
+
+
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2) / a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+ Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-
+ (double)67.*e6/(double)3024.);
+ Ra0 = Ra * (double)0.4222382;
+ Ra1 = Ra * (double)1.3265004;
+
+ theta = phi / (double)2.;
+ dtheta = (double)1.;
+ tol = (double)4.85e-10;
+ phis = sin(phi);
+
+ dlam = lambda - M0;
+ if(dlam>GPS_PI)
+ dlam -= p2;
+ if(dlam<-GPS_PI)
+ dlam += p2;
+
+ while(fabs(dtheta)>tol)
+ {
+ thetas = sin(theta);
+ thetac = cos(theta);
+ n = theta+thetas*thetac+(double)2.*thetas;
+ dtheta = -(n-((double)2.+po2)*phis) /
+ ((double)2.*thetac*((double)1.+thetac));
+ theta += dtheta;
+ }
+
+ *E = Ra0*dlam*((double)1.+cos(theta))+E0;
+ *N = Ra1*sin(theta)+N0;
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_EckertIV_EN_To_LatLon **********************************
+**
+** Convert Eckert IV equal area elliptical pseudocylindrical projection
+** easting and northing to latitude and longitude
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_EckertIV_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double M0, double E0,
+ double N0, double a, double b)
+{
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double Ra0;
+ double Ra1;
+ double po2;
+ double p2;
+
+ double Ra;
+ double theta;
+ double thetas;
+ double thetac;
+ double n;
+ double dx;
+ double dy;
+ double i;
+
+
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ p2 = (double)GPS_PI * (double)2.;
+ po2 = (double)GPS_PI / (double)2.;
+
+ if(M0>GPS_PI)
+ M0-=p2;
+
+
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2) / a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+ Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-
+ (double)67.*e6/(double)3024.);
+ Ra0 = Ra * (double)0.4222382;
+ Ra1 = Ra * (double)1.3265004;
+
+ dx = E - E0;
+ dy = N - N0;
+ i = dy/Ra1;
+ if(i>(double)1.)
+ i=(double)1.;
+ else if(i<(double)-1.)
+ i=(double)-1.;
+
+ theta = asin(i);
+ thetas = sin(theta);
+ thetac = cos(theta);
+ n = theta+thetas*thetac+(double)2.*thetas;
+
+ *phi = asin(n/((double)2. + po2));
+ *lambda = M0 + dx / (Ra0*((double)1.+thetac));
+
+ if(*phi>po2)
+ *phi=po2;
+ else if(*phi<-po2)
+ *phi=-po2;
+
+ if(*lambda>GPS_PI)
+ *lambda -= p2;
+ if(*lambda<-GPS_PI)
+ *lambda += p2;
+
+ if(*lambda>GPS_PI)
+ *lambda=GPS_PI;
+ else if(*lambda<-GPS_PI)
+ *lambda=-GPS_PI;
+
+ *lambda = GPS_Math_Rad_To_Deg(*lambda);
+ *phi = GPS_Math_Rad_To_Deg(*phi);
+
+ return;
+}
+
+
+
+
+
+/* @func GPS_Math_EckertVI_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to Eckert VI equal area
+** pseudocylindrical projection easting and northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_EckertVI_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double M0, double E0, double N0,
+ double a, double b)
+{
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double Ra;
+ double Rsq;
+ double IRa;
+ double po2;
+ double p2;
+
+ double phis;
+ double theta;
+ double dtheta;
+ double dlam;
+ double tol;
+
+
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+ phi = GPS_Math_Deg_To_Rad(phi);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ p2 = (double)GPS_PI * (double)2.;
+ po2 = (double)GPS_PI / (double)2.;
+
+ if(M0>GPS_PI)
+ M0-=p2;
+
+
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2) / a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+ Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-
+ (double)67.*e6/(double)3024.);
+ Rsq = Ra/pow((double)2.+GPS_PI,(double).5);
+ IRa = (double)1./Rsq;
+
+ phis = sin(phi);
+ theta = phi;
+ dtheta = (double)1.;
+ tol = (double)4.85e-10;
+
+ dlam = lambda - M0;
+ if(dlam>GPS_PI)
+ dlam -= p2;
+ if(dlam<-GPS_PI)
+ dlam += p2;
+
+ while(fabs(dtheta)>tol)
+ {
+ dtheta = -(theta+sin(theta)-((double)1.+po2)*phis) /
+ ((double)1.+cos(theta));
+ theta += dtheta;
+ }
+
+ *E = Rsq*dlam*((double)1.+cos(theta))+E0;
+ *N = (double)2.*Rsq*theta+N0;
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_EckertVI_EN_To_LatLon **********************************
+**
+** Convert Eckert VI equal area pseudocylindrical projection
+** easting and northing to latitude and longitude
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_EckertVI_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double M0, double E0,
+ double N0, double a, double b)
+{
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double Rsq;
+ double IRa;
+ double po2;
+ double p2;
+
+ double Ra;
+ double theta;
+ double dx;
+ double dy;
+ double i;
+
+
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ p2 = (double)GPS_PI * (double)2.;
+ po2 = (double)GPS_PI / (double)2.;
+
+ if(M0>GPS_PI)
+ M0-=p2;
+
+
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2) / a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+ Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-
+ (double)67.*e6/(double)3024.);
+ Rsq = Ra/pow((double)2.+GPS_PI,(double).5);
+ IRa = (double)1./Rsq;
+
+
+ dx = E - E0;
+ dy = N - N0;
+ theta = IRa * dy / (double)2.;
+ i = (theta+sin(theta)) / ((double)1.+po2);
+ if(i>(double)1.)
+ *phi = po2;
+ else if(i<(double)-1.)
+ *phi = -po2;
+ else
+ *phi= asin(i);
+ *lambda = M0 + IRa * dx / ((double)1.+cos(theta));
+
+ if(*phi>po2)
+ *phi=po2;
+ else if(*phi<-po2)
+ *phi=-po2;
+
+ if(*lambda>GPS_PI)
+ *lambda -= p2;
+ if(*lambda<-GPS_PI)
+ *lambda += p2;
+
+ if(*lambda>GPS_PI)
+ *lambda=GPS_PI;
+ else if(*lambda<-GPS_PI)
+ *lambda=-GPS_PI;
+
+ *lambda = GPS_Math_Rad_To_Deg(*lambda);
+ *phi = GPS_Math_Rad_To_Deg(*phi);
+
+ return;
+}
+
+
+
+
+
+/* @func GPS_Math_Cyled_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to cylindrical equidistant projection
+** easting and northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Cyled_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi0, double M0, double E0,
+ double N0, double a, double b)
+{
+ double p2;
+ double po2;
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double Ra;
+ double Rac;
+ double phi0c;
+
+ double dlam;
+
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+ phi = GPS_Math_Deg_To_Rad(phi);
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ p2 = (double)GPS_PI * (double)2.0;
+ po2 = (double)GPS_PI / (double)2.0;
+ if(M0>GPS_PI)
+ M0 -= p2;
+
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+
+ Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-
+ (double)67.*e6/(double)3024.);
+ phi0c = cos(phi0);
+ Rac = Ra * phi0c;
+
+ dlam = lambda - M0;
+ if(dlam>GPS_PI)
+ dlam -= p2;
+ if(dlam<-GPS_PI)
+ dlam += p2;
+
+ *E = Rac * dlam + E0;
+ *N = Ra * phi + N0;
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Cyled_EN_To_LatLon **********************************
+**
+** Convert cylindrical equidistant easting and northing projection
+** to latitude and longitude
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Cyled_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0, double M0,
+ double E0, double N0, double a, double b)
+{
+ double p2;
+ double po2;
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double Ra;
+ double Rac;
+ double phi0c;
+
+ double dx;
+ double dy;
+
+
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ p2 = (double)GPS_PI * (double)2.0;
+ po2 = (double)GPS_PI / (double)2.0;
+ if(M0>GPS_PI)
+ M0 -= p2;
+
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+
+ Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-
+ (double)67.*e6/(double)3024.);
+ phi0c = cos(phi0);
+ Rac = Ra * phi0c;
+
+ dx = E - E0;
+ dy = N - N0;
+
+ if(!Rac)
+ *lambda = (double)0.;
+ else
+ *lambda = M0 + dx / Rac;
+
+ *phi = dy/Ra;
+
+ if(*phi>po2)
+ *phi = po2;
+ else if(*phi<-po2)
+ *phi = -po2;
+
+ if(*lambda>GPS_PI)
+ *lambda -= p2;
+ if(*lambda<-GPS_PI)
+ *lambda += p2;
+
+ if(*lambda>GPS_PI)
+ *lambda = GPS_PI;
+ else if(*lambda<-GPS_PI)
+ *lambda=-GPS_PI;
+
+ *lambda = GPS_Math_Rad_To_Deg(*lambda);
+ *phi = GPS_Math_Rad_To_Deg(*phi);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_VderGrinten_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to Van der Grinten polyconic projection
+** easting and northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_VderGrinten_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double M0, double E0,
+ double N0, double a, double b)
+{
+ double po2;
+ double p2;
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double Ra;
+ double pRa;
+
+ double gg;
+ double pp;
+ double pp2;
+ double gm0;
+ double ppa;
+ double thetai;
+ double theta;
+ double thetas;
+ double thetac;
+ double qq;
+ double tol;
+ double aa;
+ double aa2;
+ double dlam;
+
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+ phi = GPS_Math_Deg_To_Rad(phi);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ p2 = (double)GPS_PI * (double)2.0;
+ po2 = (double)GPS_PI / (double)2.0;
+ if(M0>GPS_PI)
+ M0 -= p2;
+
+
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+
+ Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-(double)67.*
+ e6/(double)3024.);
+ pRa = (double)GPS_PI * Ra;
+
+ dlam = lambda - M0;
+ if(dlam>GPS_PI)
+ dlam -= p2;
+ if(dlam<-GPS_PI)
+ dlam += p2;
+
+ tol = (double)1.0e-5;
+
+ if(!phi)
+ {
+ *N = (double)0.0;
+ *E = Ra*dlam+E0;
+ }
+ else if(!dlam || (((po2-tol)<tol)&&(phi<(po2+tol))) ||
+ (((-po2-tol)<tol)&&(phi<(-po2+tol))))
+ {
+ thetai = fabs(((double)2./(double)GPS_PI) * phi);
+ if(thetai>(double)1.)
+ thetai=(double)1.;
+ else if(thetai<(double)-1.)
+ thetai=(double)-1.;
+
+ theta = asin(thetai);
+ *E = 0;
+ *N = pRa * tan(theta/(double)2.) * N0;
+ if(phi<(double)0.0)
+ *N *= (double)-1.;
+ }
+ else
+ {
+ aa = (double).5*fabs((double)GPS_PI/dlam - dlam/(double)GPS_PI);
+ thetai = fabs(((double)2./(double)GPS_PI) * phi);
+ if(thetai>(double)1.)
+ thetai=(double)1.;
+ else if(thetai<(double)-1.)
+ thetai=(double)-1.;
+
+ theta = asin(thetai);
+ thetas = sin(theta);
+ thetac = cos(theta);
+ gg = thetac/(thetas+thetac-(double)1.);
+ pp = gg*((double)2./thetas-(double)1.);
+ aa2 = aa*aa;
+ pp2 = pp*pp;
+ gm0 = gg-pp2;
+ ppa = pp2+aa2;
+ qq = aa2+gg;
+ *E = pRa*(aa*gm0+pow(aa2*gm0*gm0-ppa*(gg*gg-pp2),(double).5))/ppa+E0;
+ if(dlam<(double)0.0)
+ *E *= (double)-1.;
+ *N = pRa*(pp*qq-aa*pow((aa2+(double)1.)*ppa-qq*qq,(double).5))/ppa+N0;
+ if(phi<(double)0.0)
+ *N *= (double)-1.;
+ }
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_VderGrinten_EN_To_LatLon **********************************
+**
+** Convert Van der Grinten polyconic easting and northing projection
+** to latitude and longitude
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_VderGrinten_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double M0, double E0,
+ double N0, double a, double b)
+{
+ double po2;
+ double p2;
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double Ra;
+ double pRa;
+
+ double dx;
+ double dy;
+ double xx;
+ double xx2;
+ double yy;
+ double yy2;
+ double tyy2;
+ double xpy;
+ double c1;
+ double c2;
+ double c3;
+ double c3c3;
+ double co;
+ double dd;
+ double a1;
+ double m1;
+ double i;
+ double theta;
+
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ p2 = (double)GPS_PI * (double)2.0;
+ po2 = (double)GPS_PI / (double)2.0;
+ if(M0>GPS_PI)
+ M0 -= p2;
+
+
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+
+ Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-(double)67.*
+ e6/(double)3024.);
+ pRa = (double)GPS_PI * Ra;
+
+
+ dx = E - E0;
+ dy = N - N0;
+ xx = dx/pRa;
+ yy = dy/pRa;
+ xx2 = xx*xx;
+ yy2 = yy*yy;
+ xpy = xx2+yy2;
+ tyy2 = yy2*(double)2.;
+
+ if(!N)
+ *phi=(double)0.0;
+ else
+ {
+ c1 = -fabs(yy)*((double)1.+xpy);
+ c2 = c1-tyy2+xx2;
+ c3 = (double)-2.*c1+(double)1.+tyy2+xpy*xpy;
+ co = c2/((double)3.*c3);
+ c3c3 = c3*c3;
+ dd = yy2/c3+(((double)2.*c2*c2*c2)/(c3c3*c3)-((double)9.*c1*c2)/
+ c3c3)/(double)27.;
+ a1 = (c1-c2*co)/c3;
+ m1 = (double)2.* pow(-((double)1./(double)3.)*a1,(double).5);
+ i = (double)3.*dd/(a1*m1);
+ if((i>(double)1.)||(i<(double)-1.))
+ *phi=po2;
+ else
+ {
+ theta = ((double)1./(double)3.)*acos((double)3.*dd/(a1*m1));
+ *phi = (double)GPS_PI*(-m1*cos(theta+(double)GPS_PI/(double)3.)-
+ co);
+ }
+ }
+
+ if(N<(double)0.0)
+ *phi *= (double)-1.0;
+
+ if(!xx)
+ *lambda = M0;
+ else
+ *lambda = (double)GPS_PI * (xpy-(double)1.+
+ pow((double)1.+((double)2.*xx2-tyy2)+xpy*xpy,(double).5)) /
+ ((double)2.*xx) + M0;
+ if(*phi>po2)
+ *phi = po2;
+ else if(*phi<-po2)
+ *phi = -po2;
+
+ if(*lambda>GPS_PI)
+ *lambda -= p2;
+ if(*lambda<-GPS_PI)
+ *lambda += p2;
+
+ if(*lambda>GPS_PI)
+ *lambda = GPS_PI;
+ else if(*lambda<-GPS_PI)
+ *lambda=-GPS_PI;
+
+ *lambda = GPS_Math_Rad_To_Deg(*lambda);
+ *phi = GPS_Math_Rad_To_Deg(*phi);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Bonne_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to Bonne pseudoconic equal area projection
+** easting and northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] phi1 [double] latitude of true scale (deg)
+** @param [r] lambda1 [double] longitude from pole (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_PolarSt_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi1, double lambda1,
+ double E0, double N0, double a, double b)
+{
+ double p2;
+ double po2;
+ double a2;
+ double b2;
+ double e2;
+ double e4=(double)0.;
+ double e;
+ double eo2;
+ double sh;
+ double mc;
+ double tc=(double)0.;
+ double amc=(double)0.;
+ double ta;
+ double phi1s;
+ double phi1c;
+ double es;
+ double op;
+ double om;
+ double pe;
+ double polat;
+ double polon;
+
+ double dlam;
+ double phis;
+ double t;
+ double rho;
+
+
+ lambda1 = GPS_Math_Deg_To_Rad(lambda1);
+ phi1 = GPS_Math_Deg_To_Rad(phi1);
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+ phi = GPS_Math_Deg_To_Rad(phi);
+
+
+ p2 = (double)GPS_PI * (double)2.0;
+ po2 = (double)GPS_PI / (double)2.0;
+
+ ta = a * (double)2.0;
+ if(lambda1>GPS_PI)
+ lambda1 -= p2;
+ if(phi1<(double)0.0)
+ {
+ sh=(double)1.0;
+ polat = -phi1;
+ polon = -lambda1;
+ }
+ else
+ {
+ sh=(double)0.0;
+ polat = phi1;
+ polon = lambda1;
+ }
+
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e = pow(e2,(double).5);
+ eo2 = e/(double)2.;
+
+ if(fabs(fabs(polat)-po2)>(double)1.0e-10)
+ {
+ phi1s = sin(polat);
+ phi1c = cos(polat);
+ es = e*phi1s;
+ pe = pow(((double)1.-es)/((double)1.+es),eo2);
+ mc = phi1c / pow((double)1.-es*es,(double).5);
+ amc = mc * a;
+ tc = tan(((double)GPS_PI/(double)4.)-polat/(double)2.) / pe;
+ }
+ else
+ {
+ op = (double)1. + e;
+ om = (double)1. - e;
+ e4 = pow(pow(op,op)*pow(om,om),(double).5);
+ }
+
+
+
+ if(fabs(fabs(phi)-po2)<(double)1.0e-10)
+ *E = *N = (double)0.0;
+ else
+ {
+ if(sh)
+ {
+ phi *= (double)-1.0;
+ lambda *= (double)-1.0;
+ }
+
+ dlam = lambda - polon;
+ if(dlam>GPS_PI)
+ dlam -= p2;
+ if(dlam<-GPS_PI)
+ dlam += p2;
+ phis = sin(phi);
+ es = e * phis;
+ pe = pow(((double)1.-es)/((double)1.+es),eo2);
+ t = tan(((double)GPS_PI/(double)4.)-phi/(double)2.) / pe;
+
+ if(fabs(fabs(polat)-po2)>(double)1.0e-10)
+ rho = amc * t / tc;
+ else
+ rho = ta * t / e4;
+ *E = rho * sin(dlam) + E0;
+
+ if(sh)
+ {
+ *E *= (double)-1.;
+ *N = rho * cos(dlam) + N0;
+ }
+ else
+ *N = -rho * cos(dlam) + N0;
+ }
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_PolarSt_EN_To_LatLon **********************************
+**
+** Convert Polar Stereographic easting and northing projection
+** to latitude and longitude
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] phi1 [double] latitude of true scale (deg)
+** @param [r] lambda1 [double] longitude from pole (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_PolarSt_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi1, double lambda1,
+ double E0, double N0, double a, double b)
+{
+ double p2;
+ double po2;
+ double a2;
+ double b2;
+ double e2;
+ double e4=(double)0.;
+ double e;
+ double eo2;
+ double sh;
+ double mc;
+ double tc=(double)0.;
+ double amc=(double)0.;
+ double ta;
+ double phi1s;
+ double phi1c;
+ double es;
+ double op;
+ double om;
+ double pe;
+ double polat;
+ double polon;
+
+ double dx;
+ double dy;
+ double t;
+ double rho;
+ double PHI;
+ double PHIS;
+ double TPHI;
+
+
+ lambda1 = GPS_Math_Deg_To_Rad(lambda1);
+ phi1 = GPS_Math_Deg_To_Rad(phi1);
+
+
+ p2 = (double)GPS_PI * (double)2.0;
+ po2 = (double)GPS_PI / (double)2.0;
+
+ ta = a * (double)2.0;
+ if(lambda1>GPS_PI)
+ lambda1 -= p2;
+ if(phi1<(double)0.0)
+ {
+ sh=(double)1.0;
+ polat = -phi1;
+ polon = -lambda1;
+ }
+ else
+ {
+ sh=(double)0.0;
+ polat = phi1;
+ polon = lambda1;
+ }
+
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e = pow(e2,(double).5);
+ eo2 = e/(double)2.;
+
+ if(fabs(fabs(polat)-po2)>(double)1.0e-10)
+ {
+ phi1s = sin(polat);
+ phi1c = cos(polat);
+ es = e*phi1s;
+ pe = pow(((double)1.-es)/((double)1.+es),eo2);
+ mc = phi1c / pow((double)1.-es*es,(double).5);
+ amc = mc * a;
+ tc = tan(((double)GPS_PI/(double)4.)-polat/(double)2.) / pe;
+ }
+ else
+ {
+ op = (double)1. + e;
+ om = (double)1. - e;
+ e4 = pow(pow(op,op)*pow(om,om),(double).5);
+ }
+
+
+ dx = E - E0;
+ dy = N - N0;
+ if(!dx && !dy)
+ {
+ *phi = po2;
+ *lambda = polon;
+ }
+ else
+ {
+ if(sh)
+ {
+ dx *= (double)-1.;
+ dy *= (double)-1.;
+ }
+ rho = pow(dx*dx+dy*dy,(double).5);
+ if(fabs(fabs(polat)-po2)>(double)1.0e-10)
+ t = rho * tc / amc;
+ else
+ t = rho * e4 / ta;
+ TPHI = (double)0.0;
+ PHI = po2 - (double)2.*atan(t);
+ while(fabs(PHI-TPHI)>(double)1.0e-10)
+ {
+ TPHI=PHI;
+ PHIS = sin(PHI);
+ es = e * PHIS;
+ pe = pow(((double)1.-es)/((double)1.+es),eo2);
+ PHI = po2 - (double)2. * atan(t*pe);
+ }
+ *phi = PHI;
+ *lambda = polon + atan2(dx,-dy);
+
+ if(*phi>po2)
+ *phi = po2;
+ else if(*phi<-po2)
+ *phi = -po2;
+
+ if(*lambda>GPS_PI)
+ *lambda -= p2;
+ if(*lambda<-GPS_PI)
+ *lambda += p2;
+
+ if(*lambda>GPS_PI)
+ *lambda = GPS_PI;
+ else if(*lambda<-GPS_PI)
+ *lambda=-GPS_PI;
+ }
+ if(sh)
+ {
+ *phi *= (double)-1.;
+ *lambda *= (double)1.;
+ }
+
+ *lambda = GPS_Math_Rad_To_Deg(*lambda);
+ *phi = GPS_Math_Rad_To_Deg(*phi);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Mollweide_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to Mollweide projection easting and
+** northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Mollweide_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double M0, double E0,
+ double N0, double a, double b)
+{
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double p2;
+ double po2;
+ double Ra;
+ double sRa2;
+ double sRa8;
+
+ double ps;
+ double dlam;
+ double theta;
+ double thetap;
+ double d;
+ double tol;
+
+ phi = GPS_Math_Deg_To_Rad(phi);
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ po2 = (double)GPS_PI / (double)2.0;
+ p2 = (double)GPS_PI * (double)2.0;
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e4*e2;
+
+ Ra = a*((double)1.0-e2/(double)6.0-(double)17.0*e4/(double)360.0-
+ (double)67.0*e6/(double)3024.0);
+ sRa2 = pow((double)2.,(double).5) * Ra;
+ sRa8 = pow((double)8.,(double).5) * Ra;
+
+ if(M0>GPS_PI)
+ M0 -= p2;
+
+ ps = sin(phi) * (double)GPS_PI;
+ d = (double)0.1745329;
+ tol = (double)4.85e-10;
+ thetap = phi;
+
+ dlam = lambda - M0;
+ if(dlam>GPS_PI)
+ dlam-=p2;
+ if(dlam<-GPS_PI)
+ dlam+=p2;
+
+ while(fabs(d)>tol)
+ {
+ d = -(thetap+sin(thetap)-ps)/((double)1.+cos(thetap));
+ thetap += d;
+ }
+ theta = thetap / (double)2.;
+ *E = (sRa8/(double)GPS_PI) * dlam * cos(theta) + E0;
+ *N = sRa2 * sin(theta) + N0;
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Mollweide_EN_To_LatLon **********************************
+**
+** Convert latitude and longitude to Mollweide projection easting and
+** northing
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Mollweide_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double M0, double E0,
+ double N0, double a, double b)
+{
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double p2;
+ double po2;
+ double Ra;
+ double sRa2;
+ double sRa8;
+
+ double dx;
+ double dy;
+ double theta=(double)0.;
+ double tt;
+ double i;
+
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ po2 = (double)GPS_PI / (double)2.0;
+ p2 = (double)GPS_PI * (double)2.0;
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e4*e2;
+
+ Ra = a*((double)1.0-e2/(double)6.0-(double)17.0*e4/(double)360.0-
+ (double)67.0*e6/(double)3024.0);
+ sRa2 = pow((double)2.,(double).5) * Ra;
+ sRa8 = pow((double)8.,(double).5) * Ra;
+
+ if(M0>GPS_PI)
+ M0 -= p2;
+
+ dx = E - E0;
+ dy = N - N0;
+ i = dy/sRa2;
+ if(fabs(i)>(double)1.)
+ {
+ *phi = po2;
+ if(N<(double)0.0)
+ *phi *= (double)-1.;
+ }
+ else
+ {
+ theta = asin(i);
+ tt = theta * (double)2.;
+ *phi = asin((tt+sin(tt))/(double)GPS_PI);
+ if(*phi>po2)
+ *phi=po2;
+ else if (*phi<-po2)
+ *phi=-po2;
+ }
+
+ if(fabs(fabs(*phi)-po2)<(double)1.0e-10)
+ *lambda = M0;
+ else
+ *lambda = M0 + (double)GPS_PI * dx / (sRa8 * cos(theta));
+
+
+ if(*lambda>GPS_PI)
+ *lambda-=p2;
+ if(*lambda<-GPS_PI)
+ *lambda+=p2;
+
+ if(*lambda>GPS_PI)
+ *lambda=GPS_PI;
+ else if(*lambda<-GPS_PI)
+ *lambda=-GPS_PI;
+
+ *lambda = GPS_Math_Rad_To_Deg(*lambda);
+ *phi = GPS_Math_Rad_To_Deg(*phi);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Orthog_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to orthographic projection
+** easting and northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] lambda0 [double] longitude of origin (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Orthog_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi0, double lambda0,
+ double E0, double N0, double a, double b)
+{
+ double p2;
+ double po2;
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double Ra;
+ double phi0s;
+ double phi0c;
+
+ double phis;
+ double phic;
+ double dlam;
+ double clc;
+ double cc;
+
+
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+ phi = GPS_Math_Deg_To_Rad(phi);
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ lambda0 = GPS_Math_Deg_To_Rad(lambda0);
+
+ p2 = (double)GPS_PI * (double)2.0;
+ po2 = (double)GPS_PI / (double)2.0;
+ if(lambda0>GPS_PI)
+ lambda0 -= p2;
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+ Ra = a * ((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-(double)67.*
+ e6/(double)3024.);
+ phi0s = sin(phi0);
+ phi0c = cos(phi0);
+
+ dlam = lambda - lambda0;
+ if(dlam>GPS_PI)
+ dlam -= p2;
+ if(dlam<-GPS_PI)
+ dlam += p2;
+
+
+ phis = sin(phi);
+ phic = cos(phi);
+ clc = phic * cos(dlam);
+ cc = phi0s * phis + phi0c * clc;
+
+ *E = Ra * phic * sin(dlam) + E0;
+ *N = Ra * (phi0c * phis - phi0s * clc) + N0;
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Orthog_EN_To_LatLon **********************************
+**
+** Convert Orthogonal easting and northing projection
+** to latitude and longitude
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] lambda0 [double] longitude of origin (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Orthog_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0, double lambda0,
+ double E0, double N0, double a, double b)
+{
+ double p2;
+ double po2;
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double Ra;
+ double phi0s;
+ double phi0c;
+
+ double dx;
+ double dy;
+ double rho;
+ double adod;
+ double ror;
+ double cc;
+ double ccs;
+ double ccc;
+
+
+
+
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ lambda0 = GPS_Math_Deg_To_Rad(lambda0);
+
+ p2 = (double)GPS_PI * (double)2.0;
+ po2 = (double)GPS_PI / (double)2.0;
+ if(lambda0>GPS_PI)
+ lambda0 -= p2;
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+ Ra = a * ((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-(double)67.*
+ e6/(double)3024.);
+ phi0s = sin(phi0);
+ phi0c = cos(phi0);
+
+
+ dx = E - E0;
+ dy = N - N0;
+ adod = atan(dx/dy);
+ rho = pow(dx*dx+dy*dy,(double).5);
+ if(!rho)
+ {
+ *phi = phi0;
+ *lambda = lambda0;
+ }
+ else
+ {
+ ror = rho/Ra;
+ if(ror>(double)1.)
+ ror=(double)1.;
+ else if(ror<(double)-1.)
+ ror=(double)-1.;
+ cc = asin(ror);
+ ccs = sin(cc);
+ ccc = cos(cc);
+ *phi = asin(ccc*phi0s+(dy*ccs*phi0c/rho));
+ if(phi0==po2)
+ *lambda = lambda0 - adod;
+ else if(phi0==-po2)
+ *lambda = lambda0 + adod;
+ else
+ *lambda = lambda0+atan(dx*ccs/(rho*phi0c*ccc-dy*phi0s*ccs));
+ }
+
+ if(*phi>po2)
+ *phi = po2;
+ else if(*phi<-po2)
+ *phi = -po2;
+
+ if(*lambda>GPS_PI)
+ *lambda -= p2;
+ if(*lambda<-GPS_PI)
+ *lambda += p2;
+
+ if(*lambda>GPS_PI)
+ *lambda = GPS_PI;
+ else if(*lambda<-GPS_PI)
+ *lambda=-GPS_PI;
+
+ *lambda = GPS_Math_Rad_To_Deg(*lambda);
+ *phi = GPS_Math_Rad_To_Deg(*phi);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Polycon_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to Polyconic projection
+** easting and northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Polycon_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi0, double M0,
+ double E0, double N0, double a, double b)
+{
+ double p2;
+ double po2;
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double AM0;
+ double c0;
+ double c1;
+ double c2;
+ double c3;
+ double j;
+ double te4;
+ double phi0s2;
+ double phi0s4;
+ double phi0s6;
+
+ double phis;
+ double phis2;
+ double phis4;
+ double phis6;
+ double dlam;
+ double NN;
+ double NNot;
+ double MM;
+ double EE;
+ double lat;
+
+
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+ phi = GPS_Math_Deg_To_Rad(phi);
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ p2 = (double)GPS_PI * (double)2.0;
+ po2 = (double)GPS_PI / (double)2.0;
+ if(M0>GPS_PI)
+ M0 -= p2;
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+
+ j = (double)45.0*e6/(double)1024.0;
+ te4 = (double)3.0 * e4;
+ c0 = (double)1.0-e2/(double)4.0-te4/(double)64.0-(double)5.0*e6/
+ (double)256.0;
+ c1 = (double)3.0*e2/(double)8.0+te4/(double)32.0+j;
+ c2 = (double)15.0*e4/(double)256.0+j;
+ c3 = (double)35.0*e6/(double)3072.0;
+
+ lat = c0 * phi0;
+
+ phi0s2 = c1 * sin((double)2.0*phi0);
+ phi0s4 = c2 * sin((double)4.0*phi0);
+ phi0s6 = c3 * sin((double)6.0*phi0);
+ AM0 = a*(lat-phi0s2+phi0s4-phi0s6);
+
+
+
+ dlam = lambda - M0;
+ if(dlam>GPS_PI)
+ dlam -= p2;
+ if(dlam<-GPS_PI)
+ dlam += p2;
+
+ phis = sin(phi);
+
+ if(!phi)
+ {
+ *E = a * dlam + E0;
+ *N = -AM0 + N0;
+ }
+ else
+ {
+ NN = a / pow((double)1.-e2*phis*phis,(double).5);
+ NNot = NN / tan(phi);
+ lat = c0 * phi;
+ phis2 = c1 * sin((double)2.0*phi);
+ phis4 = c2 * sin((double)4.0*phi);
+ phis6 = c3 * sin((double)6.0*phi);
+ MM = a*(lat-phis2+phis4-phis6);
+ EE = dlam *phis;
+ *E = NNot * sin(EE) + E0;
+ *N = MM - AM0 + NNot * ((double)1.-cos(EE)) + N0;
+ }
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Polycon_EN_To_LatLon **********************************
+**
+** Convert Polyconic easting and northing projection
+** to latitude and longitude
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Polycon_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0, double M0,
+ double E0, double N0, double a, double b)
+{
+ double p2;
+ double po2;
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double AM0;
+ double c0;
+ double c1;
+ double c2;
+ double c3;
+ double j;
+ double te4;
+ double phi0s2;
+ double phi0s4;
+ double phi0s6;
+
+ double dx;
+ double dy;
+ double dxoa;
+ double AA;
+ double BB;
+ double CC=(double)0.;
+ double PHIn;
+ double PHId;
+ double PHIs;
+ double PHI;
+ double PHIs2;
+ double PHIs4;
+ double PHIs6;
+ double Mn;
+ double Mnp;
+ double Ma;
+ double AAMa;
+ double mpb;
+ double AAmin;
+ double tol;
+ double lat;
+
+
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ p2 = (double)GPS_PI * (double)2.0;
+ po2 = (double)GPS_PI / (double)2.0;
+ if(M0>GPS_PI)
+ M0 -= p2;
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+
+ j = (double)45.0*e6/(double)1024.0;
+ te4 = (double)3.0 * e4;
+ c0 = (double)1.0-e2/(double)4.0-te4/(double)64.0-(double)5.0*e6/
+ (double)256.0;
+ c1 = (double)3.0*e2/(double)8.0+te4/(double)32.0+j;
+ c2 = (double)15.0*e4/(double)256.0+j;
+ c3 = (double)35.0*e6/(double)3072.0;
+
+ lat = c0 * phi0;
+
+ phi0s2 = c1 * sin((double)2.0*phi0);
+ phi0s4 = c2 * sin((double)4.0*phi0);
+ phi0s6 = c3 * sin((double)6.0*phi0);
+ AM0 = a*(lat-phi0s2+phi0s4-phi0s6);
+
+ tol = (double)1.0e-12;
+
+ dx = E - E0;
+ dy = N - N0;
+ dxoa = dx/a;
+ if((((-AM0-(double)1.)<dy)&&(dy<(-AM0+(double)1.))))
+ {
+ *phi = (double)0.;
+ *lambda = dxoa + M0;
+ }
+ else
+ {
+ AA = (AM0+dy) / a;
+ BB = dxoa * dxoa + (AA*AA);
+ PHIn = AA;
+ PHId = (double)1.;
+
+ while(fabs(PHId)>tol)
+ {
+ PHIs = sin(PHIn);
+ CC = pow((double)1.-e2*PHIs*PHIs,(double).5) * tan(PHIn);
+ PHI = PHIn * c0;
+ PHIs2 = c1 * sin((double)2.0*PHIn);
+ PHIs4 = c2 * sin((double)4.0*PHIn);
+ PHIs6 = c3 * sin((double)6.0*PHIn);
+ Mn = a*(PHI-PHIs2+PHIs4-PHIs6);
+ Mnp = c0 - (double)2.*c1*cos((double)2.*PHIn)+(double)4.*c2*
+ cos((double)4.*PHIn)-(double)6.*c3*cos((double)6.*PHIn);
+ Ma = Mn / a;
+ AAMa = AA * Ma;
+ mpb = Ma*Ma+BB;
+ AAmin = AA - Ma;
+ PHId = (AAMa*CC+AAmin-(double).5*mpb*CC)/
+ (e2*PHIs2*(mpb-(double)2.*AAMa) /
+ (double)4.*CC+AAmin*(CC*Mnp-(double)2./PHIs2)-Mnp);
+ PHIn -= PHId;
+ }
+ *phi = PHIn;
+
+ if(*phi>po2)
+ *phi = po2;
+ else if(*phi<-po2)
+ *phi = -po2;
+
+ if((((po2-(double).00001)<fabs(*phi))&&
+ (fabs(*phi)<(po2+(double).00001))))
+ *lambda = M0;
+ else
+ *lambda = (asin(dxoa*CC)) / sin(*phi) + M0;
+ }
+
+ if(*lambda>GPS_PI)
+ *lambda -= p2;
+ if(*lambda<-GPS_PI)
+ *lambda += p2;
+
+ if(*lambda>GPS_PI)
+ *lambda = GPS_PI;
+ else if(*lambda<-GPS_PI)
+ *lambda=-GPS_PI;
+
+ *lambda = GPS_Math_Rad_To_Deg(*lambda);
+ *phi = GPS_Math_Rad_To_Deg(*phi);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Sinusoid_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to Sinusoidal projection easting and
+** northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Sinusoid_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double M0, double E0,
+ double N0, double a, double b)
+{
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double p2;
+ double po2;
+ double c0;
+ double c1;
+ double c2;
+ double c3;
+ double A1;
+ double A0;
+ double A2;
+ double A3;
+ double E1;
+ double E2;
+ double E3;
+ double E4;
+ double j;
+ double om0;
+ double som0;
+
+ double phis;
+ double phis2;
+ double phis4;
+ double phis6;
+ double mm;
+ double MM;
+ double dlam;
+
+
+ phi = GPS_Math_Deg_To_Rad(phi);
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ po2 = (double)GPS_PI / (double)2.0;
+ p2 = (double)GPS_PI * (double)2.0;
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e4*e2;
+
+ j = (double)45.*e6/(double)1024.;
+ c0 = (double)1.-e2/(double)4.-(double)3.*e4/(double)64.-(double)5.*
+ e6/(double)256.;
+ c1 = (double)3.*e2/(double)8.+(double)3.*e4/(double)32.+j;
+ c2 = (double)15.*e4/(double)256.+j;
+ c3 = (double)35.*e6/(double)3072.;
+ om0 = (double)1. - e2;
+ som0 = pow(om0,(double).5);
+ E1 = ((double)1.-som0)/((double)1.+som0);
+ E2 = E1*E1;
+ E3 = E1*E2;
+ E4 = E1*E3;
+ A0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.;
+ A1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.;
+ A2 = (double)151.*E3/(double)96.;
+ A3 = (double)1097.*E4/(double)512.;
+
+ if(M0>GPS_PI)
+ M0 -= p2;
+
+ phis = sin(phi);
+
+ dlam = lambda - M0;
+ if(dlam>GPS_PI)
+ dlam-=p2;
+ if(dlam<-GPS_PI)
+ dlam+=p2;
+
+ mm = pow((double)1.-e2*phis*phis,(double).5);
+ phis2 = c1 * sin((double)2.*phi);
+ phis4 = c2 * sin((double)4.*phi);
+ phis6 = c3 * sin((double)6.*phi);
+ MM = a * (c0*phi-phis2+phis4-phis6);
+
+
+
+ *E = a*dlam*cos(phi)/mm+E0;
+ *N = MM + N0;
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Sinusoid_EN_To_LatLon **********************************
+**
+** Convert latitude and longitude to Sinusoidal projection easting and
+** northing
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Sinusoid_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double M0, double E0,
+ double N0, double a, double b)
+{
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e6;
+ double p2;
+ double po2;
+ double c0;
+ double c1;
+ double c2;
+ double c3;
+ double A1;
+ double A0;
+ double A2;
+ double A3;
+ double E1;
+ double E2;
+ double E3;
+ double E4;
+ double j;
+ double om0;
+ double som0;
+
+ double dx;
+ double dy;
+ double mu;
+ double mu2s;
+ double mu4s;
+ double mu6s;
+ double mu8s;
+ double phis;
+
+
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ po2 = (double)GPS_PI / (double)2.0;
+ p2 = (double)GPS_PI * (double)2.0;
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e4*e2;
+
+ j = (double)45.*e6/(double)1024.;
+ c0 = (double)1.-e2/(double)4.-(double)3.*e4/(double)64.-(double)5.*
+ e6/(double)256.;
+ c1 = (double)3.*e2/(double)8.+(double)3.*e4/(double)32.+j;
+ c2 = (double)15.*e4/(double)256.+j;
+ c3 = (double)35.*e6/(double)3072.;
+ om0 = (double)1. - e2;
+ som0 = pow(om0,(double).5);
+ E1 = ((double)1.-som0)/((double)1.+som0);
+ E2 = E1*E1;
+ E3 = E1*E2;
+ E4 = E1*E3;
+ A0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.;
+ A1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.;
+ A2 = (double)151.*E3/(double)96.;
+ A3 = (double)1097.*E4/(double)512.;
+
+
+ dx = E - E0;
+ dy = N - N0;
+
+ mu = dy/(c0*a);
+ mu2s = A0 * sin((double)2.*mu);
+ mu4s = A1 * sin((double)4.*mu);
+ mu6s = A2 * sin((double)6.*mu);
+ mu8s = A3 * sin((double)8.*mu);
+ *phi = mu + mu2s + mu4s + mu6s + mu8s;
+
+ if(*phi>po2)
+ *phi=po2;
+ else if (*phi<-po2)
+ *phi=-po2;
+
+ if((((po2-(double)1.0e-8)<fabs(*phi))&&(fabs(*phi)<(po2+(double)1.0e-8))))
+ *lambda = M0;
+ else
+ {
+ phis = sin(*phi);
+ *lambda = M0 + dx*pow((double)1.0-e2*phis*phis,(double).5) /
+ (a*cos(*phi));
+ }
+
+ if(*lambda>GPS_PI)
+ *lambda-=p2;
+ if(*lambda<-GPS_PI)
+ *lambda+=p2;
+
+ if(*lambda>GPS_PI)
+ *lambda=GPS_PI;
+ else if(*lambda<-GPS_PI)
+ *lambda=-GPS_PI;
+
+ *lambda = GPS_Math_Rad_To_Deg(*lambda);
+ *phi = GPS_Math_Rad_To_Deg(*phi);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_TCylEA_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to transverse cylindrical equal area
+** projection easting and northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_TCylEA_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi0, double M0, double E0,
+ double N0, double a, double b)
+{
+ double p2;
+ double po2;
+ double a2;
+ double b2;
+ double e;
+ double e2;
+ double e4;
+ double e6;
+ double AM0;
+ double qp;
+ double om;
+ double oo;
+ double c0;
+ double c1;
+ double c2;
+ double c3;
+ double b0;
+ double b1;
+ double B2;
+ double b3;
+ double A0;
+ double A1;
+ double A2;
+ double sf;
+ double x;
+ double som;
+ double phis;
+ double j;
+ double te4;
+ double lat;
+ double phi0s2;
+ double phi0s4;
+ double phi0s6;
+ double E1;
+ double E2;
+ double E3;
+ double E4;
+
+ double dlam;
+ double qq;
+ double qqo;
+ double bt;
+ double btc;
+ double PHI;
+ double PHIs2;
+ double PHIs4;
+ double PHIs6;
+ double bts2;
+ double bts4;
+ double bts6;
+ double PHIc;
+ double PHIcs;
+ double Mc;
+
+
+
+ sf = (double)1.0; /* scale factor */
+
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+ phi = GPS_Math_Deg_To_Rad(phi);
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ p2 = (double)GPS_PI * (double)2.0;
+ po2 = (double)GPS_PI / (double)2.0;
+ if(M0>GPS_PI)
+ M0 -= p2;
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+ e = pow(e2,(double).5);
+ om = (double)1.-e2;
+ som = pow(om,(double).5);
+ oo = (double)1./((double)2.*e);
+
+ phis = sin(po2);
+ x = e * phis;
+ qp = om*(phis/((double)1.-e2*phis*phis)-oo*
+ log(((double)1.-x)/((double)1.+x)));
+
+ A0 = e2 / (double)3.+(double)31.*e4/(double)180.+(double)517.*
+ e6/(double)5040.;
+ A1 = (double)23.*e4/(double)360.+(double)251.*e6/(double)3780.;
+ A2 = (double)761.*e6/(double)45360.;
+
+ E1 = ((double)1.0-som) / ((double)1.0+som);
+ E2 = E1*E1;
+ E3 = E2*E1;
+ E4 = E3*E1;
+
+ b0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.;
+ b1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.;
+ B2 = (double)151.*E3/(double)96.;
+ b3 = (double)1097.*E4/(double)512.;
+
+
+ j = (double)45.0*e6/(double)1024.0;
+ te4 = (double)3.0 * e4;
+ c0 = (double)1.0-e2/(double)4.0-te4/(double)64.0-(double)5.0*e6/
+ (double)256.0;
+ c1 = (double)3.0*e2/(double)8.0+te4/(double)32.0+j;
+ c2 = (double)15.0*e4/(double)256.0+j;
+ c3 = (double)35.0*e6/(double)3072.0;
+
+ lat = c0 * phi0;
+
+ phi0s2 = c1 * sin((double)2.0*phi0);
+ phi0s4 = c2 * sin((double)4.0*phi0);
+ phi0s6 = c3 * sin((double)6.0*phi0);
+ AM0 = a*(lat-phi0s2+phi0s4-phi0s6);
+
+
+ dlam = lambda - M0;
+ if(dlam>GPS_PI)
+ dlam -= p2;
+ if(dlam<-GPS_PI)
+ dlam += p2;
+
+ phis = sin(phi);
+
+ if(phi==po2)
+ {
+ qq = qp;
+ qqo = (double)1.;
+ }
+ else
+ {
+ x = e * phis;
+ qq = om*(phis/((double)1.-e2*phis*phis)-oo*
+ log(((double)1.-x)/((double)1.+x)));
+ qqo = qq/qp;
+ }
+
+ if(qqo>(double)1.)
+ qqo = (double)1.;
+ else if(qqo<(double)-1.)
+ qqo = (double)-1.;
+
+ bt = asin(qqo);
+ btc = atan(tan(bt)/cos(dlam));
+
+ if((fabs(btc)-po2)>(double)1.0e-8)
+ PHIc = btc;
+ else
+ {
+ bts2 = A0 * sin((double)2.0*btc);
+ bts4 = A1 * sin((double)4.0*btc);
+ bts6 = A2 * sin((double)6.0*btc);
+ PHIc = btc + bts2 + bts4 + bts6;
+ }
+
+ PHIcs = sin(PHIc);
+ *E = a*cos(bt)*cos(PHIc)*sin(dlam)/(sf*cos(btc)*
+ pow((double)1.-e2*PHIcs*PHIcs,
+ (double).5)) + E0;
+ PHI = c0 * PHIc;
+ PHIs2 = c1 * sin((double)2.0*PHIc);
+ PHIs4 = c2 * sin((double)4.0*PHIc);
+ PHIs6 = c3 * sin((double)6.0*PHIc);
+ Mc = a*(PHI-PHIs2+PHIs4-PHIs6);
+
+ *N = sf * (Mc-AM0) + N0;
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_TCylEA_EN_To_LatLon **********************************
+**
+** Convert transverse cylindrical equal area easting and northing projection
+** to latitude and longitude
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] M0 [double] central meridian (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_TCylEA_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0, double M0,
+ double E0, double N0, double a, double b)
+{
+ double p2;
+ double po2;
+ double a2;
+ double b2;
+ double e;
+ double e2;
+ double e4;
+ double e6;
+ double AM0;
+ double qp;
+ double om;
+ double oo;
+ double c0;
+ double c1;
+ double c2;
+ double c3;
+ double b0;
+ double b1;
+ double B2;
+ double b3;
+ double A0;
+ double A1;
+ double A2;
+ double sf;
+ double x;
+ double som;
+ double phis;
+ double j;
+ double te4;
+ double lat;
+ double phi0s2;
+ double phi0s4;
+ double phi0s6;
+ double E1;
+ double E2;
+ double E3;
+ double E4;
+
+ double dx;
+ double dy;
+ double bt;
+ double btc;
+ double btp;
+ double btcc;
+ double Mc;
+ double Muc;
+ double mus2;
+ double mus4;
+ double mus6;
+ double mus8;
+ double bts2;
+ double bts4;
+ double bts6;
+ double PHIc;
+ double Qc;
+ double Qco;
+ double t;
+
+
+ sf = (double)1.0; /* scale factor */
+
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ M0 = GPS_Math_Deg_To_Rad(M0);
+
+ p2 = (double)GPS_PI * (double)2.0;
+ po2 = (double)GPS_PI / (double)2.0;
+ if(M0>GPS_PI)
+ M0 -= p2;
+ a2 = a*a;
+ b2 = b*b;
+ e2 = (a2-b2)/a2;
+ e4 = e2*e2;
+ e6 = e2*e4;
+ e = pow(e2,(double).5);
+ om = (double)1.-e2;
+ som = pow(om,(double).5);
+ oo = (double)1./((double)2.*e);
+
+ phis = sin(po2);
+ x = e * phis;
+ qp = om*(phis/((double)1.-e2*phis*phis)-oo*
+ log(((double)1.-x)/((double)1.+x)));
+
+ A0 = e2 / (double)3.+(double)31.*e4/(double)180.+(double)517.*
+ e6/(double)5040.;
+ A1 = (double)23.*e4/(double)360.+(double)251.*e6/(double)3780.;
+ A2 = (double)761.*e6/(double)45360.;
+
+ E1 = ((double)1.0-som) / ((double)1.0+som);
+ E2 = E1*E1;
+ E3 = E2*E1;
+ E4 = E3*E1;
+
+ b0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.;
+ b1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.;
+ B2 = (double)151.*E3/(double)96.;
+ b3 = (double)1097.*E4/(double)512.;
+
+
+ j = (double)45.0*e6/(double)1024.0;
+ te4 = (double)3.0 * e4;
+ c0 = (double)1.0-e2/(double)4.0-te4/(double)64.0-(double)5.0*e6/
+ (double)256.0;
+ c1 = (double)3.0*e2/(double)8.0+te4/(double)32.0+j;
+ c2 = (double)15.0*e4/(double)256.0+j;
+ c3 = (double)35.0*e6/(double)3072.0;
+
+ lat = c0 * phi0;
+
+ phi0s2 = c1 * sin((double)2.0*phi0);
+ phi0s4 = c2 * sin((double)4.0*phi0);
+ phi0s6 = c3 * sin((double)6.0*phi0);
+ AM0 = a*(lat-phi0s2+phi0s4-phi0s6);
+
+
+
+ dx = E - E0;
+ dy = N - N0;
+ Mc = AM0 + dy/sf;
+ Muc = Mc / (c0*a);
+
+ mus2 = b0 * sin((double)2.0*Muc);
+ mus4 = b1 * sin((double)4.0*Muc);
+ mus6 = B2 * sin((double)6.0*Muc);
+ mus8 = b3 * sin((double)6.0*Muc);
+ PHIc = Muc + mus2 + mus4 + mus6 + mus8;
+
+ phis = sin(PHIc);
+ x = e * phis;
+ Qc = om*(phis/((double)1.-e2*phis*phis)-oo*
+ log(((double)1.-x)/((double)1.+x)));
+ Qco = Qc/qp;
+
+ if(Qco>(double)1.)
+ Qco = (double)1.;
+ else if(Qco<(double)-1.)
+ Qco = (double)-1.;
+
+ btc = asin(Qco);
+ btcc = cos(btc);
+ t = sf*dx*btcc*pow((double)1.-e2*phis*phis,(double).5)/(a*cos(PHIc));
+ if(t>(double)1.)
+ t=(double)1.;
+ else if(t<(double)-1.)
+ t=(double)-1.;
+ btp = -asin(t);
+ bt = asin(cos(btp)*sin(btc));
+
+ bts2 = A0 * sin((double)2.0*bt);
+ bts4 = A1 * sin((double)4.0*bt);
+ bts6 = A2 * sin((double)6.0*bt);
+ *phi = bt + bts2 + bts4 + bts6;
+ *lambda = M0 - atan(tan(btp)/btcc);
+
+ if(*phi>po2)
+ *phi = po2;
+ else if(*phi<-po2)
+ *phi = -po2;
+
+ if(*lambda>GPS_PI)
+ *lambda -= p2;
+ if(*lambda<-GPS_PI)
+ *lambda += p2;
+
+ if(*lambda>GPS_PI)
+ *lambda = GPS_PI;
+ else if(*lambda<-GPS_PI)
+ *lambda=-GPS_PI;
+
+ *lambda = GPS_Math_Rad_To_Deg(*lambda);
+ *phi = GPS_Math_Rad_To_Deg(*phi);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Mercator_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to standard Mercator projection
+** easting and northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] lambda0 [double] longitude of origin (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Mercator_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi0, double lambda0,
+ double E0, double N0, double a, double b)
+{
+ double p2;
+ double po2;
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e3;
+ double e;
+ double es;
+ double ab;
+ double bb;
+ double cb;
+ double db;
+ double ml;
+ double phi0s;
+ double sf;
+
+ double dlam;
+ double ct;
+ double ex;
+ double tt;
+ double pt;
+
+
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+ phi = GPS_Math_Deg_To_Rad(phi);
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ lambda0 = GPS_Math_Deg_To_Rad(lambda0);
+
+ ml = ((double)GPS_PI*(double)89.5)/(double)180.;
+
+ p2 = (double)GPS_PI * (double)2.0;
+ po2 = (double)GPS_PI / (double)2.0;
+ if(lambda0>GPS_PI)
+ lambda0 -= p2;
+ a2 = a*a;
+ b2 = b*b;
+ es = (a2-b2)/a2;
+ e2 = es*es;
+ e3 = e2*es;
+ e4 = e3*es;
+
+ e = pow(es,(double).5);
+ phi0s = sin(phi0);
+ sf = (double)1. / (pow((double)1.-es*phi0s*phi0s,(double).5)/cos(phi0));
+
+ ab = es/(double)2.+(double)5.*e2/(double)24.+e3/(double)12.+(double)13.*
+ e4/(double)360.;
+ bb = (double)7.*e2/(double)48.+(double)29.*e3/(double)240.+
+ (double)811.*e4/(double)11520.;
+ cb = (double)7.*e3/(double)120.+(double)81.*e4/(double)1120.;
+ db = (double)4279.*e4/(double)161280.;
+
+
+
+ if(lambda>(double)GPS_PI)
+ lambda -= p2;
+
+ dlam = lambda - lambda0;
+ if(dlam>GPS_PI)
+ dlam -= p2;
+ if(dlam<-GPS_PI)
+ dlam += p2;
+
+
+ ex = e * sin(phi);
+ tt = tan((double)GPS_PI/(double)4.+phi/(double)2.);
+ pt = pow((((double)1.-ex)/((double)1.+ex)),(e/(double)2.));
+
+ ct = tt * pt;
+ *N = sf * a * log(ct) + N0;
+ *E = sf * a * dlam + E0;
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Mercator_EN_To_LatLon **********************************
+**
+** Convert standard Mercator easting and northing projection
+** to latitude and longitude
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] lambda0 [double] longitude of origin (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_Mercator_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0,
+ double lambda0, double E0, double N0,
+ double a, double b)
+{
+ double p2;
+ double po2;
+ double a2;
+ double b2;
+ double e2;
+ double e4;
+ double e3;
+ double e;
+ double es;
+ double ab;
+ double bb;
+ double cb;
+ double db;
+ double ml;
+ double phi0s;
+ double sf;
+
+ double dx;
+ double dy;
+ double x;
+
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ lambda0 = GPS_Math_Deg_To_Rad(lambda0);
+
+ ml = ((double)GPS_PI*(double)89.5)/(double)180.;
+
+ p2 = (double)GPS_PI * (double)2.0;
+ po2 = (double)GPS_PI / (double)2.0;
+ if(lambda0>GPS_PI)
+ lambda0 -= p2;
+ a2 = a*a;
+ b2 = b*b;
+ es = (a2-b2)/a2;
+ e2 = es*es;
+ e3 = e2*es;
+ e4 = e3*es;
+
+ e = pow(es,(double).5);
+ phi0s = sin(phi0);
+ sf = (double)1. / (pow((double)1.-es*phi0s*phi0s,(double).5)/cos(phi0));
+
+ ab = es/(double)2.+(double)5.*e2/(double)24.+e3/(double)12.+(double)13.*
+ e4/(double)360.;
+ bb = (double)7.*e2/(double)48.+(double)29.*e3/(double)240.+
+ (double)811.*e4/(double)11520.;
+ cb = (double)7.*e3/(double)120.+(double)81.*e4/(double)1120.;
+ db = (double)4279.*e4/(double)161280.;
+
+ dx = E - E0;
+ dy = N - N0;
+ *lambda = lambda0 + dx / (sf*a);
+ x = (double)GPS_PI / (double)2. -
+ (double)2.*atan((double)1./exp(dy/(sf*a)));
+ *phi = x+ab*sin((double)2.*x)+bb*sin((double)4.*x)+cb*sin((double)6.*x)
+ + db*sin((double)8.*x);
+
+ if(*phi>po2)
+ *phi = po2;
+ else if(*phi<-po2)
+ *phi = -po2;
+
+ if(*lambda>GPS_PI)
+ *lambda -= p2;
+ if(*lambda<-GPS_PI)
+ *lambda += p2;
+
+ if(*lambda>GPS_PI)
+ *lambda = GPS_PI;
+ else if(*lambda<-GPS_PI)
+ *lambda=-GPS_PI;
+
+ *lambda = GPS_Math_Rad_To_Deg(*lambda);
+ *phi = GPS_Math_Rad_To_Deg(*phi);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_TMerc_LatLon_To_EN **********************************
+**
+** Convert latitude and longitude to transverse Mercator projection
+** easting and northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] lambda0 [double] longitude of origin (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] F0 [double] scale factor
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_TMerc_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi0, double lambda0,
+ double E0, double N0, double F0,
+ double a, double b)
+{
+ GPS_Math_LatLon_To_EN(E,N,phi,lambda,N0,E0,phi0,lambda0,F0,a,b);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_TMerc_EN_To_LatLon **********************************
+**
+** Convert transverse Mercator easting and northing projection
+** to latitude and longitude
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] phi0 [double] latitude of origin (deg)
+** @param [r] lambda0 [double] longitude of origin (deg)
+** @param [r] E0 [double] false easting
+** @param [r] N0 [double] false northing
+** @param [r] F0 [double] scale factor
+** @param [r] a [double] semi-major axis
+** @param [r] b [double] semi-minor axis
+**
+** @return [void]
+************************************************************************/
+void GPS_Math_TMerc_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0, double lambda0,
+ double E0, double N0, double F0,
+ double a, double b)
+{
+ GPS_Math_EN_To_LatLon(E,N,phi,lambda,N0,E0,phi0,lambda0,F0,a,b);
+
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Swiss_LatLon_To_EN ***********************************
+**
+** Convert latitude and longitude to Swiss grid easting and northing
+**
+** @param [r] phi [double] latitude (deg)
+** @param [r] lambda [double] longitude (deg)
+** @param [w] E [double *] easting (metre)
+** @param [w] N [double *] northing (metre)
+** @param [r] phi0 [double] latitude origin (deg) [normally 46.95240556]
+** @param [r] lambda0 [double] longitude origin (deg) [normally 7.43958333]
+** @param [r] E0 [double] false easting (metre) [normally 600000.0]
+** @param [r] N0 [double] false northing (metre) [normally 200000.0]
+** @param [r] a [double] semi-major axis [normally 6377397.000]
+** @param [r] b [double] semi-minor axis [normally 6356078.823]
+**
+** @return [void]
+***************************************************************************/
+void GPS_Math_Swiss_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N,double phi0,double lambda0,
+ double E0, double N0, double a, double b)
+
+{
+ double a2;
+ double b2;
+ double esq;
+ double e;
+ double c;
+ double ephi0p;
+ double phip;
+ double sphip;
+ double phid;
+ double slambda2;
+ double lambda1;
+ double lambda2;
+ double K;
+ double po4;
+ double w;
+ double R;
+
+ lambda0 = GPS_Math_Deg_To_Rad(lambda0);
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+ lambda = GPS_Math_Deg_To_Rad(lambda);
+ phi = GPS_Math_Deg_To_Rad(phi);
+
+ po4=GPS_PI/(double)4.0;
+
+ a2 = a*a;
+ b2 = b*b;
+ esq = (a2-b2)/a2;
+ e = pow(esq,(double)0.5);
+
+ c = sqrt(1+((esq*pow(cos(phi0),(double)4.))/((double)1.-esq)));
+
+ ephi0p = asin(sin(phi0)/c);
+
+ K = log(tan(po4+ephi0p/(double)2.)) - c*(log(tan(po4+phi0/(double)2.)) -
+ e/(double)2. * log(((double)1.+e*sin(phi0)) /
+ ((double)1.-e*sin(phi0))));
+ lambda1 = c*(lambda-lambda0);
+ w = c*(log(tan(po4+phi/(double)2.)) - e/(double)2. *
+ log(((double)1.+e*sin(phi)) / ((double)1.-e*sin(phi)))) + K;
+
+
+ phip = (double)2. * (atan(exp(w)) - po4);
+
+ sphip = cos(ephi0p) * sin(phip) - sin(ephi0p) * cos(phip) * cos(lambda1);
+ phid = asin(sphip);
+
+ slambda2 = cos(phip)*sin(lambda1) / cos(phid);
+ lambda2 = asin(slambda2);
+
+ R = a*sqrt((double)1.-esq) / ((double)1.-esq*sin(phi0) * sin(phi0));
+
+ *N = R*log(tan(po4 + phid/(double)2.)) + N0;
+ *E = R*lambda2 + E0;
+ return;
+}
+
+
+
+
+/* @func GPS_Math_Swiss_EN_To_LatLon ************************************
+**
+** Convert Swiss Grid easting and northing to latitude and longitude
+**
+** @param [r] E [double] easting (metre)
+** @param [r] N [double] northing (metre)
+** @param [w] phi [double *] latitude (deg)
+** @param [w] lambda [double *] longitude (deg)
+** @param [r] phi0 [double] latitude origin (deg) [normally 46.95240556]
+** @param [r] lambda0 [double] longitude origin (deg) [normally 7.43958333]
+** @param [r] E0 [double] false easting (metre) [normally 600000.0]
+** @param [r] N0 [double] false northing (metre) [normally 200000.0]
+** @param [r] a [double] semi-major axis [normally 6377397.000]
+** @param [r] b [double] semi-minor axis [normally 6356078.823]
+**
+** @return [void]
+*************************************************************************/
+
+void GPS_Math_Swiss_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0, double lambda0,
+ double E0, double N0, double a, double b)
+{
+ double a2;
+ double b2;
+ double esq;
+ double e;
+ double R;
+ double c;
+ double po4;
+ double phid;
+ double phi1;
+ double lambdad;
+ double lambda1;
+ double slambda1;
+ double ephi0p;
+ double sphip;
+ double tol;
+ double cr;
+ double C;
+ double K;
+
+ lambda0 = GPS_Math_Deg_To_Rad(lambda0);
+ phi0 = GPS_Math_Deg_To_Rad(phi0);
+
+ po4=GPS_PI/(double)4.0;
+ tol=(double)0.00001;
+
+ a2 = a*a;
+ b2 = b*b;
+ esq = (a2-b2)/a2;
+ e = pow(esq,(double)0.5);
+
+ R = a*sqrt((double)1.-esq) / ((double)1.-esq*sin(phi0) * sin(phi0));
+
+ phid = (double)2.*(atan(exp((N - N0)/R)) - po4);
+ lambdad = (E - E0)/R;
+
+ c = sqrt((double)1.+((esq * pow(cos(phi0), (double)4.)) /
+ ((double)1.-esq)));
+ ephi0p = asin(sin(phi0) / c);
+
+ sphip = cos(ephi0p)*sin(phid) + sin(ephi0p)*cos(phid)*cos(lambdad);
+ phi1 = asin(sphip);
+
+ slambda1 = cos(phid)*sin(lambdad)/cos(phi1);
+ lambda1 = asin(slambda1);
+
+ *lambda = GPS_Math_Rad_To_Deg((lambda1/c + lambda0));
+
+ K = log(tan(po4 + ephi0p/(double)2.)) -c*(log(tan(po4 + phi0/(double)2.))
+ - e/(double)2. * log(((double)1.+e*sin(phi0)) /
+ ((double)1.-e*sin(phi0))));
+ C = (K - log(tan(po4 + phi1/(double)2.)))/c;
+
+ do
+ {
+ cr = (C + log(tan(po4 + phi1/(double)2.)) - e/(double)2. *
+ log(((double)1.+e*sin(phi1)) / ((double)1.-e*sin(phi1)))) *
+ ((((double)1.-esq*sin(phi1)*sin(phi1)) * cos(phi1)) /
+ ((double)1.-esq));
+ phi1 -= cr;
+ }
+ while (fabs(cr) > tol);
+
+ *phi = GPS_Math_Rad_To_Deg(phi1);
+
+ return;
+}
--- /dev/null
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef gpsproj_h
+#define gpsproj_h
+
+
+#include "gps.h"
+
+void GPS_Math_Albers_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi1, double phi2,
+ double phi0, double M0, double E0,
+ double N0, double a, double b);
+void GPS_Math_Albers_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi1, double phi2,
+ double phi0, double M0, double E0,
+ double N0, double a, double b);
+
+
+void GPS_Math_LambertCC_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi1, double phi2,
+ double phi0, double M0, double E0,
+ double N0, double a, double b);
+void GPS_Math_LambertCC_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi1, double phi2,
+ double phi0, double M0, double E0,
+ double N0, double a, double b);
+
+void GPS_Math_Miller_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double M0, double E0,
+ double N0, double a, double b);
+void GPS_Math_Miller_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double M0, double E0,
+ double N0, double a, double b);
+
+void GPS_Math_Bonne_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi0, double M0, double E0,
+ double N0, double a, double b);
+void GPS_Math_Bonne_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0, double M0,
+ double E0, double N0, double a, double b);
+
+void GPS_Math_Cassini_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi0, double M0,
+ double E0, double N0, double a, double b);
+void GPS_Math_Cassini_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0, double M0,
+ double E0, double N0, double a, double b);
+
+void GPS_Math_Cylea_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi0, double M0,
+ double E0, double N0, double a, double b);
+void GPS_Math_Cylea_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0, double M0,
+ double E0, double N0, double a, double b);
+
+void GPS_Math_EckertIV_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double M0, double E0, double N0,
+ double a, double b);
+void GPS_Math_EckertIV_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double M0, double E0,
+ double N0, double a, double b);
+
+void GPS_Math_EckertVI_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double M0, double E0, double N0,
+ double a, double b);
+void GPS_Math_EckertVI_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double M0, double E0,
+ double N0, double a, double b);
+
+void GPS_Math_Cyled_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi0, double M0, double E0,
+ double N0, double a, double b);
+void GPS_Math_Cyled_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0, double M0,
+ double E0, double N0, double a, double b);
+
+void GPS_Math_VderGrinten_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double M0, double E0,
+ double N0, double a, double b);
+void GPS_Math_VderGrinten_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double M0, double E0,
+ double N0, double a, double b);
+
+void GPS_Math_PolarSt_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi1, double lambda1,
+ double E0, double N0, double a, double b);
+void GPS_Math_PolarSt_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi1, double lambda1,
+ double E0, double N0, double a, double b);
+
+void GPS_Math_Mollweide_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double M0, double E0,
+ double N0, double a, double b);
+void GPS_Math_Mollweide_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double M0, double E0,
+ double N0, double a, double b);
+
+void GPS_Math_Orthog_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi0, double lambda0,
+ double E0, double N0, double a, double b);
+void GPS_Math_Orthog_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0, double lambda0,
+ double E0, double N0, double a, double b);
+
+void GPS_Math_Polycon_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi0, double M0,
+ double E0, double N0, double a, double b);
+void GPS_Math_Polycon_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0, double M0,
+ double E0, double N0, double a, double b);
+
+void GPS_Math_Sinusoid_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double M0, double E0,
+ double N0, double a, double b);
+void GPS_Math_Sinusoid_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double M0, double E0,
+ double N0, double a, double b);
+
+void GPS_Math_TCylEA_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi0, double M0, double E0,
+ double N0, double a, double b);
+void GPS_Math_TCylEA_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0, double M0,
+ double E0, double N0, double a, double b);
+
+void GPS_Math_Mercator_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi0, double lambda0,
+ double E0, double N0, double a, double b);
+void GPS_Math_Mercator_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0,
+ double lambda0, double E0, double N0,
+ double a, double b);
+
+void GPS_Math_TMerc_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N, double phi0, double lambda0,
+ double E0, double N0, double F0,
+ double a, double b);
+void GPS_Math_TMerc_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0, double lambda0,
+ double E0, double N0, double F0,
+ double a, double b);
+
+void GPS_Math_Swiss_LatLon_To_EN(double phi, double lambda, double *E,
+ double *N,double phi0,double lambda0,
+ double E0, double N0, double a, double b);
+void GPS_Math_Swiss_EN_To_LatLon(double E, double N, double *phi,
+ double *lambda, double phi0, double lambda0,
+ double E0, double N0, double a, double b);
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null
+/********************************************************************
+** @source JEEPS protocol table lookup functions (GPS' without A001)
+**
+** @author Copyright (C) 1999 Alan Bleasby
+** @version 1.0
+** @modified Dec 28 1999 Alan Bleasby. First version
+** @@
+**
+** This library is free software; you can redistribute it and/or
+** modify it under the terms of the GNU Library General Public
+** License as published by the Free Software Foundation; either
+** version 2 of the License, or (at your option) any later version.
+**
+** This library is distributed in the hope that it will be useful,
+** but WITHOUT ANY WARRANTY; without even the implied warranty of
+** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+** Library General Public License for more details.
+**
+** You should have received a copy of the GNU Library General Public
+** License along with this library; if not, write to the
+** Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+** Boston, MA 02111-1307, USA.
+********************************************************************/
+#include "gps.h"
+#include <stdio.h>
+
+#define GPS_TAGUNK 20
+
+/* Storage for any unknown tags */
+static int32 gps_tag_unknown[GPS_TAGUNK];
+static int32 gps_tag_data_unknown[GPS_TAGUNK];
+static int32 gps_n_tag_unknown = 0;
+
+
+
+struct COMMANDDATA COMMAND_ID[2]=
+{
+ {
+ 0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x31,0x32
+ }
+ ,
+ {
+ 0x00,0x04,0x00,0x00,0x08,0x14,0x00,0x15,0x1a,0x00,0x00
+ }
+};
+
+struct LINKDATA LINK_ID[3]=
+{
+ {
+ 0x06,0,0,0,0,0,0x15,0,0,0,0,0,0,0,0,0,0xfd,0xfe,0xff
+ }
+ ,
+ {
+ 0x06,0x0a,0x0c,0x0e,0x11,0x13,0x15,0x1b,0x1d,0x1e,
+ 0x1f,0x22,0x23,0x33,0x62,0x63,0xfd,0xfe,0xff
+ }
+ ,
+ {
+ 0x06,0x0b,0x0c,0x14,0x18,0,0x15,0x23,0x25,0x27,0x04,
+ 0,0x2b,0,0,0,0xfd,0xfe,0xff
+ }
+};
+
+struct GPS_MODEL_PROTOCOL GPS_MP[]=
+{
+ { 7,pL001,pA010,pA100,pD100,pA200,pD200,-1,-1,-1,-1,
+ pA500,pD500
+ },
+ { 13,pL001,pA010,pA100,pD100,pA200,pD200,pD100,pA300,pD300,pA400,pD400,
+ pA500,pD500
+ },
+ { 14,pL001,pA010,pA100,pD100,pA200,pD200,pD100,-1,-1,pA400,pD400,
+ pA500,pD500
+ },
+ { 15,pL001,pA010,pA100,pD151,pA200,pD200,pD151,-1,-1,pA400,pD151,
+ pA500,pD500
+ },
+ { 18,pL001,pA010,pA100,pD100,pA200,pD200,pD100,pA300,pD300,pA400,pD400,
+ pA500,pD500
+ },
+ { 20,pL002,pA011,pA100,pD150,pA200,pD201,pD150,-1,-1,pA400,pD450,
+ pA500,pD550
+ },
+ { 22,pL001,pA010,pA100,pD152,pA200,pD200,pD152,pA300,pD300,pA400,pD152,
+ pA500,pD500
+ },
+ { 23,pL001,pA010,pA100,pD100,pA200,pD200,pD100,pA300,pD300,pA400,pD400,
+ pA500,pD500
+ },
+ { 24,pL001,pA010,pA100,pD100,pA200,pD200,pD100,pA300,pD300,pA400,pD400,
+ pA500,pD500
+ },
+ { 25,pL001,pA010,pA100,pD100,pA200,pD200,pD100,pA300,pD300,pA400,pD400,
+ pA500,pD500
+ },
+ { 29,pL001,pA010,pA100,pD101,pA200,pD201,pD101,pA300,pD300,pA400,pD101,
+ pA500,pD500
+ },
+ { 929,pL001,pA010,pA100,pD102,pA200,pD201,pD102,pA300,pD300,pA400,pD102,
+ pA500,pD500
+ },
+ { 31,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,-1,-1,
+ pA500,pD500
+ },
+ { 33,pL002,pA011,pA100,pD150,pA200,pD201,pD150,-1,-1,pA400,pD450,
+ pA500,pD550
+ },
+ { 34,pL002,pA011,pA100,pD150,pA200,pD201,pD150,-1,-1,pA400,pD450,
+ pA500,pD550
+ },
+ { 35,pL001,pA010,pA100,pD100,pA200,pD200,pD100,pA300,pD300,pA400,pD400,
+ pA500,pD500
+ },
+ { 36,pL001,pA010,pA100,pD152,pA200,pD200,pD152,pA300,pD300,pA400,pD152,
+ pA500,pD500
+ },
+ { 936,pL001,pA010,pA100,pD152,pA200,pD200,pD152,pA300,pD300,-1,-1,
+ pA500,pD500
+ },
+ { 39,pL001,pA010,pA100,pD151,pA200,pD201,pD151,pA300,pD300,-1,-1,
+ pA500,pD500
+ },
+ { 41,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,-1,-1,
+ pA500,pD500
+ },
+ { 42,pL001,pA010,pA100,pD100,pA200,pD200,pD100,pA300,pD300,pA400,pD400,
+ pA500,pD500
+ },
+ { 44,pL001,pA010,pA100,pD101,pA200,pD201,pD101,pA300,pD300,pA400,pD101,
+ pA500,pD500
+ },
+ { 45,pL001,pA010,pA100,pD152,pA200,pD201,pD152,pA300,pD300,-1,-1,
+ pA500,pD500
+ },
+ { 47,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,-1,-1,
+ pA500,pD500
+ },
+ { 48,pL001,pA010,pA100,pD154,pA200,pD201,pD154,pA300,pD300,-1,-1,
+ pA500,pD501
+ },
+ { 49,pL001,pA010,pA100,pD102,pA200,pD201,pD102,pA300,pD300,pA400,pD102,
+ pA500,pD501
+ },
+ { 50,pL001,pA010,pA100,pD152,pA200,pD201,pD152,pA300,pD300,-1,-1,
+ pA500,pD501
+ },
+ { 52,pL002,pA011,pA100,pD150,pA200,pD201,pD150,-1,-1,pA400,pD450,
+ pA500,pD550
+ },
+ { 53,pL001,pA010,pA100,pD152,pA200,pD201,pD152,pA300,pD300,-1,-1,
+ pA500,pD501
+ },
+ { 55,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,-1,-1,
+ pA500,pD500
+ },
+ { 56,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,-1,-1,
+ pA500,pD500
+ },
+ { 59,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,-1,-1,
+ pA500,pD500
+ },
+ { 61,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,-1,-1,
+ pA500,pD500
+ },
+ { 62,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,-1,-1,
+ pA500,pD500
+ },
+ { 64,pL002,pA011,pA100,pD150,pA200,pD201,pD150,-1,-1,pA400,pD450,
+ pA500,pD551
+ },
+ { 71,pL001,pA010,pA100,pD155,pA200,pD201,pD155,pA300,pD300,-1,-1,
+ pA500,pD501
+ },
+ { 72,pL001,pA010,pA100,pD104,pA200,pD201,pD104,pA300,pD300,-1,-1,
+ pA500,pD501
+ },
+ { 73,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,-1,-1,
+ pA500,pD501
+ },
+ { 74,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,-1,-1,
+ pA500,pD500
+ },
+ { 76,pL001,pA010,pA100,pD102,pA200,pD201,pD102,pA300,pD300,pA400,pD102,
+ pA500,pD501
+ },
+ { 77,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,pA400,pD400,
+ pA500,pD501
+ },
+ { 777,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,pA400,pD403,
+ pA500,pD501
+ },
+ { 877,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,-1,-1,
+ pA500,pD501
+ },
+ { 977,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,pA400,pD403,
+ pA500,pD501
+ },
+ { 87,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,pA400,pD403,
+ pA500,pD501
+ },
+ { 88,pL001,pA010,pA100,pD102,pA200,pD201,pD102,pA300,pD300,pA400,pD102,
+ pA500,pD501
+ },
+ { 95,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,pA400,pD403,
+ pA500,pD501
+ },
+ { 96,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,pA400,pD403,
+ pA500,pD501
+ },
+ { 97,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,-1,-1,
+ pA500,pD501
+ },
+ { 98,pL002,pA011,pA100,pD150,pA200,pD201,pD150,-1,-1,pA400,pD450,
+ pA500,pD551
+ },
+ { 100,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,pA400,pD403,
+ pA500,pD501
+ },
+ { 105,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,pA400,pD403,
+ pA500,pD501
+ },
+ { 106,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,pA400,pD403,
+ pA500,pD501
+ },
+ { 112,pL001,pA010,pA100,pD152,pA200,pD201,pD152,pA300,pD300,-1,-1,
+ pA500,pD501
+ },
+ { 0,0,0,0,0,0,0,0,0,0,0,0,0,0
+ }
+};
+
+
+/* @func GPS_Protocol_Version_Change ************************************
+**
+** Alters/recalculates ID, if necessary, for indexing the GPS_MP
+** structure in order to find available protocols.
+**
+** @param [r] id [US] Garmin id
+** @param [r] version [UD] Garmin version
+**
+** @return [void]
+************************************************************************/
+
+US GPS_Protocol_Version_Change(US id, US version)
+{
+ if(id==29)
+ if(version>=400)
+ id = 929;
+
+ if(id==36)
+ if(version>=300)
+ id = 936;
+
+ if(id==77)
+ {
+ if(version>=301 && version<350)
+ id = 777;
+ else if(version>=350 && version<361)
+ id = 877;
+ else if(version>=361)
+ id = 977;
+ }
+
+ return id;
+}
+
+
+
+/* @func GPS_Protocol_Table_Set ************************************
+**
+** Set protocol capabilities based on table look-up
+** For those units without the A001 protocol
+**
+** @param [r] id [const US] id
+**
+** @return [int32] Success
+************************************************************************/
+
+int32 GPS_Protocol_Table_Set(US id)
+{
+ int32 i;
+ US v;
+ char s[GPS_ARB_LEN];
+
+ i=0;
+ while((v=GPS_MP[i].id))
+ {
+ if(v==id)
+ {
+ gps_link_type = GPS_MP[i].link;
+ gps_device_command = GPS_MP[i].command-10;
+ gps_waypt_transfer = GPS_MP[i].wayptt;
+ gps_waypt_type = GPS_MP[i].wayptd;
+ gps_route_transfer = GPS_MP[i].rtea;
+ gps_rte_hdr_type = GPS_MP[i].rted0;
+ gps_rte_type = GPS_MP[i].rted1;
+ gps_trk_transfer = GPS_MP[i].trka;
+ gps_trk_type = GPS_MP[i].trkd;
+ gps_prx_waypt_transfer = GPS_MP[i].prxa;
+ gps_prx_waypt_type = GPS_MP[i].prxd;
+ gps_almanac_transfer = GPS_MP[i].alma;
+ gps_almanac_type = GPS_MP[i].almd;
+ return 1;
+ }
+ ++i;
+ }
+
+
+ (void)sprintf(s,"INIT: No table entry for ID %d\n",id);
+ GPS_Error(s);
+
+ return GPS_UNSUPPORTED;
+}
+
+
+/* @func GPS_Protocol_Error *******************************************
+**
+** Called if an unrecognised/illegal protocol is met
+** For those units with the A001 protocol
+**
+** @param [r] tag [const US] tag
+** @param [r] data [const US] data
+**
+** @return [void]
+************************************************************************/
+
+void GPS_Protocol_Error(US tag, US data)
+{
+ char s[GPS_ARB_LEN];
+
+ (void) sprintf(s,"PROTOCOL ERROR: Unknown tag/data [%c/%d]\n",tag,data);
+ GPS_Error(s);
+
+ if(gps_n_tag_unknown < GPS_TAGUNK)
+ {
+ gps_tag_unknown[gps_n_tag_unknown] = tag;
+ gps_tag_data_unknown[gps_n_tag_unknown++] = data;
+ }
+
+ return;
+}
+
+
+
+/* @func GPS_Unknown_Protocol_Print *******************************************
+**
+** Diagnostic routine for printing out any unknown protocols
+** For those units with the A001 protocol
+**
+** @return [void]
+************************************************************************/
+
+void GPS_Unknown_Protocol_Print(void)
+{
+ int32 i;
+
+ (void) fprintf(stdout,"\nUnknown protocols: ");
+ if(!gps_n_tag_unknown)
+ (void) fprintf(stdout,"None");
+ (void) fprintf(stdout,"\n");
+
+ for(i=0; i<gps_n_tag_unknown; ++i)
+ (void) fprintf(stdout,"[%c %d]\n",(char)gps_tag_unknown[i],
+ (int)gps_tag_data_unknown[i]);
+ return;
+}
--- /dev/null
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef gpsprotocols_h
+#define gpsprotocols_h
+
+#include "gps.h"
+
+/*
+ * Link protocols
+ */
+
+struct LINKDATA
+{
+ UC Pid_Ack_Byte;
+ UC Pid_Command_Data;
+ UC Pid_Xfer_Cmplt;
+ UC Pid_Date_Time_Data;
+ UC Pid_Position_Data;
+ UC Pid_Prx_Wpt_Data;
+ UC Pid_Nak_Byte;
+ UC Pid_Records;
+ UC Pid_Rte_Hdr;
+ UC Pid_Rte_Wpt_Data;
+ UC Pid_Almanac_Data;
+ UC Pid_Trk_Data;
+ UC Pid_Wpt_Data;
+ UC Pid_Pvt_Data;
+ UC Pid_Rte_Link_Data;
+ UC Pid_Trk_Hdr;
+ UC Pid_Protocol_Array;
+ UC Pid_Product_Rqst;
+ UC Pid_Product_Data;
+}
+;
+
+
+
+
+
+/*
+ * Command types
+ */
+
+#define pA010 10
+#define pA011 11
+
+int32 gps_device_command;
+
+
+struct COMMANDDATA
+{
+ UC Cmnd_Abort_Transfer;
+ UC Cmnd_Transfer_Alm;
+ UC Cmnd_Transfer_Posn;
+ UC Cmnd_Transfer_Prx;
+ UC Cmnd_Transfer_Rte;
+ UC Cmnd_Transfer_Time;
+ UC Cmnd_Transfer_Trk;
+ UC Cmnd_Transfer_Wpt;
+ UC Cmnd_Turn_Off_Pwr;
+ UC Cmnd_Start_Pvt_Data;
+ UC Cmnd_Stop_Pvt_Data;
+}
+;
+
+
+
+
+/*
+ * Waypoint Transfer Protocol
+ */
+#define pA100 100
+int32 gps_waypt_transfer;
+
+
+/*
+ * Route Transfer Protocol
+ */
+#define pA200 200
+#define pA201 201
+int32 gps_route_transfer;
+
+/*
+ * Track Log Transfer Protocol
+ */
+#define pA300 300
+#define pA301 301
+int32 gps_trk_transfer;
+
+/*
+ * Proximity Waypoint Transfer Protocol
+ */
+#define pA400 400
+int32 gps_prx_waypt_transfer;
+
+/*
+ * Almanac Transfer Protocol
+ */
+#define pA500 500
+int32 gps_almanac_transfer;
+
+
+/*
+ * Date Time Transfer
+ */
+#define pA600 600
+int32 gps_date_time_transfer;
+
+
+/*
+ * Position
+ */
+#define pA700 700
+int32 gps_position_transfer;
+
+
+/*
+ * Pvt
+ */
+#define pA800 800
+int32 gps_pvt_transfer;
+
+
+
+
+
+/*
+ * Waypoint D Type
+ */
+#define pD100 100
+#define pD101 101
+#define pD102 102
+#define pD103 103
+#define pD104 104
+#define pD105 105
+#define pD106 106
+#define pD107 107
+#define pD108 108
+#define pD150 150
+#define pD151 151
+#define pD152 152
+#define pD154 154
+#define pD155 155
+
+int32 gps_rte_type;
+int32 gps_waypt_type;
+
+
+/*
+ * Rte Header Type
+ */
+#define pD200 200
+#define pD201 201
+#define pD202 202
+int32 gps_rte_hdr_type;
+
+
+/*
+ * Rte Link Type
+ */
+#define pD210 210
+int32 gps_rte_link_type;
+
+
+/*
+ * Trk Point Type
+ */
+#define pD300 300
+#define pD301 301
+int32 gps_trk_type;
+
+
+/*
+ * Trk Header Type
+ */
+#define pD310 310
+int32 gps_trk_hdr_type;
+
+
+
+/*
+ * Prx Wpt Type
+ */
+#define pD400 400
+#define pD403 403
+#define pD450 450
+
+int32 gps_prx_waypt_type;
+
+
+/*
+ * Almanac Type
+ */
+#define pD500 500
+#define pD501 501
+#define pD550 550
+#define pD551 551
+
+int32 gps_almanac_type;
+
+
+/*
+ * Date Time Type
+ */
+#define pD600 600
+
+int32 gps_date_time_type;
+
+
+
+/*
+ * Position Type
+ */
+#define pD700 700
+
+int32 gps_position_type;
+
+
+
+/*
+ * Pvt Data Type
+ */
+#define pD800 800
+
+int32 gps_pvt_type;
+
+
+/*
+ * Link protocol type
+ */
+#define pL000 0
+#define pL001 1
+#define pL002 2
+
+int32 gps_link_type;
+
+
+
+struct GPS_MODEL_PROTOCOL
+{
+ int32 id;
+ int32 link;
+ int32 command;
+ int32 wayptt;
+ int32 wayptd;
+ int32 rtea;
+ int32 rted0;
+ int32 rted1;
+ int32 trka;
+ int32 trkd;
+ int32 prxa;
+ int32 prxd;
+ int32 alma;
+ int32 almd;
+}
+;
+
+US GPS_Protocol_Version_Change(US id, US version);
+int32 GPS_Protocol_Table_Set(US id);
+void GPS_Protocol_Error(US tag, US data);
+void GPS_Unknown_Protocol_Print(void);
+
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null
+/********************************************************************
+** @source JEEPS packet reading and acknowledging functions
+**
+** @author Copyright (C) 1999 Alan Bleasby
+** @version 1.0
+** @modified Dec 28 1999 Alan Bleasby. First version
+** @@
+**
+** This library is free software; you can redistribute it and/or
+** modify it under the terms of the GNU Library General Public
+** License as published by the Free Software Foundation; either
+** version 2 of the License, or (at your option) any later version.
+**
+** This library is distributed in the hope that it will be useful,
+** but WITHOUT ANY WARRANTY; without even the implied warranty of
+** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+** Library General Public License for more details.
+**
+** You should have received a copy of the GNU Library General Public
+** License along with this library; if not, write to the
+** Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+** Boston, MA 02111-1307, USA.
+********************************************************************/
+#include "gps.h"
+#include <stdlib.h>
+#include <stdio.h>
+#include <time.h>
+#include <errno.h>
+#include <unistd.h>
+
+
+/* @func GPS_Time_Now ***********************************************
+**
+** Get current time
+**
+** @return [time_t] number of bytes read
+**********************************************************************/
+
+time_t GPS_Time_Now(void)
+{
+ time_t secs;
+
+ if(time(&secs)==-1)
+ {
+ perror("time");
+ GPS_Error("GPS_Time_Now: Error reading time");
+ gps_errno = HARDWARE_ERROR;
+ return 0;
+ }
+
+ return secs;
+}
+
+
+
+
+
+
+
+/* @func GPS_Packet_Read ***********************************************
+**
+** Read a packet
+**
+** @param [r] fd [int32] file descriptor
+** @param [w] packet [GPS_PPacket *] packet string
+**
+** @return [int32] number of bytes read
+**********************************************************************/
+
+int32 GPS_Packet_Read(int32 fd, GPS_PPacket *packet)
+{
+ time_t start;
+ int32 n;
+ int32 len;
+ UC u;
+ int32 isDLE;
+ UC *p;
+ int32 i;
+ UC chk=0;
+
+ len = 0;
+ isDLE = gpsFalse;
+ p = (*packet)->data;
+
+ start = GPS_Time_Now();
+ while(GPS_Time_Now() < start+GPS_TIME_OUT)
+ {
+ if((n=GPS_Serial_Chars_Ready(fd)))
+ {
+ if(GPS_Serial_Read(fd,&u,1)==-1)
+ {
+ perror("read");
+ GPS_Error("GPS_Packet_Read: Read error");
+ gps_errno = FRAMING_ERROR;
+ return 0;
+ }
+
+ GPS_Diagnose(u);
+
+ if(!len)
+ {
+ (*packet)->dle = u;
+ if(u != DLE)
+ {
+ (void) fprintf(stderr,"GPS_Packet_Read: No DLE\n");
+ (void) fflush(stderr);
+ return 0;
+ }
+ ++len;
+ continue;
+ }
+
+ if(len==1)
+ {
+ (*packet)->type = u;
+ ++len;
+ continue;
+ }
+
+ if(u == DLE)
+ {
+ if(isDLE)
+ {
+ isDLE = gpsFalse;
+ continue;
+ }
+ isDLE = gpsTrue;
+ }
+
+ if(len == 2)
+ {
+ (*packet)->n = u;
+ len = -1;
+ continue;
+ }
+
+ if(u == ETX)
+ if(isDLE)
+ {
+ (*packet)->edle = DLE;
+ (*packet)->etx = ETX;
+ if(p-(*packet)->data-2 != (*packet)->n)
+ {
+ GPS_Error("GPS_Packet_Read: Bad count");
+ gps_errno = FRAMING_ERROR;
+ return 0;
+ }
+ (*packet)->chk = *(p-2);
+
+ for(i=0,p=(*packet)->data;i<(*packet)->n;++i)
+ chk -= *p++;
+ chk -= (*packet)->type;
+ chk -= (*packet)->n;
+ if(chk != (*packet)->chk)
+ {
+ GPS_Error("CHECKSUM: Read error\n");
+ gps_errno = FRAMING_ERROR;
+ return 0;
+ }
+
+ return (*packet)->n;
+ }
+
+ *p++ = u;
+ }
+ }
+
+
+ GPS_Error("GPS_Packet_Read: Time-out");
+ gps_errno = SERIAL_ERROR;
+
+ return 0;
+}
+
+
+
+/* @func GPS_Get_Ack *************************************************
+**
+** Check that returned packet is an ack for the packet sent
+**
+** @param [r] fd [int32] file descriptor
+** @param [r] tra [GPS_PPacket *] packet just transmitted
+** @param [r] rec [GPS_PPacket *] packet to receive
+**
+** @return [int32] true if ACK
+**********************************************************************/
+
+int32 GPS_Get_Ack(int32 fd, GPS_PPacket *tra, GPS_PPacket *rec)
+{
+ if(!GPS_Packet_Read(fd, rec))
+ return 0;
+
+ if(LINK_ID[0].Pid_Ack_Byte != (*rec)->type)
+ {
+ gps_error = FRAMING_ERROR;
+ return 0;
+ }
+
+ if(*(*rec)->data != (*tra)->type)
+ {
+ gps_error = FRAMING_ERROR;
+ return 0;
+ }
+
+ return 1;
+}
+
+
+
+
+
+#if 0
+
+int32 ajb(int32 fd)
+{
+ UC u;
+ int32 n;
+
+ while(1)
+ {
+ if((n=GPS_Serial_Chars_Ready(fd)))
+ {
+ if(read(fd,&u,1)==-1)
+ {
+ perror("read");
+ GPS_Error("NMEA Read: Read error");
+ gps_errno = FRAMING_ERROR;
+ return 0;
+ }
+ printf("%d %c\n",u,u);
+ }
+ }
+
+ return 0;
+}
+#endif
--- /dev/null
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef gpsread_h
+#define gpsread_h
+
+
+#include "gps.h"
+#include <time.h>
+
+time_t GPS_Time_Now(void);
+int32 GPS_Packet_Read(int32 fd, GPS_PPacket *packet);
+int32 GPS_Get_Ack(int32 fd, GPS_PPacket *tra, GPS_PPacket *rec);
+int32 ajb(int32 fd);
+
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null
+/********************************************************************
+** @source JEEPS time/position request from GPS functions
+**
+** @author Copyright (C) 1999 Alan Bleasby
+** @version 1.0
+** @modified Dec 28 1999 Alan Bleasby. First version
+** @@
+**
+** This library is free software; you can redistribute it and/or
+** modify it under the terms of the GNU Library General Public
+** License as published by the Free Software Foundation; either
+** version 2 of the License, or (at your option) any later version.
+**
+** This library is distributed in the hope that it will be useful,
+** but WITHOUT ANY WARRANTY; without even the implied warranty of
+** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+** Library General Public License for more details.
+**
+** You should have received a copy of the GNU Library General Public
+** License along with this library; if not, write to the
+** Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+** Boston, MA 02111-1307, USA.
+********************************************************************/
+#include "gps.h"
+
+
+static int32 GPS_A600_Rqst(int32 fd, time_t Time);
+static int32 GPS_A700_Rqst(int32 fd, double lat, double lon);
+
+
+
+/* @func GPS_Rqst_Send_Time ******************************************
+**
+** Set GPS time on request of GPS
+**
+** @param [r] fd [int32] file descriptor
+** @param [r] Time [time_t] unix-style time
+**
+** @return [int32] true if OK
+************************************************************************/
+
+int32 GPS_Rqst_Send_Time(int32 fd, time_t Time)
+{
+ time_t ret=0;
+
+ switch(gps_date_time_transfer)
+ {
+ case pA600:
+ ret = GPS_A600_Rqst(fd, Time);
+ break;
+ default:
+ GPS_Error("Rqst_Send_Time: Unknown date/time protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ return ret;
+}
+
+
+
+/* @funcstatic GPS_A600_Rqst *******************************************
+**
+** Send time to GPS
+**
+** @param [r] fd [int32] file descriptor
+** @param [r] Time [time_t] unix-style time
+**
+** @return [int32] success
+************************************************************************/
+static int32 GPS_A600_Rqst(int32 fd, time_t Time)
+{
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+
+ switch(gps_date_time_type)
+ {
+ case pD600:
+ GPS_D600_Send(&tra,Time);
+ break;
+ default:
+ GPS_Error("A600_Rqst: Unknown data/time protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ return 1;
+}
+
+
+
+/* @func GPS_Rqst_Send_Position ******************************************
+**
+** Set GPS position
+**
+** @param [r] fd [int32] filedescriptor
+** @param [r] lat [double] latitude (deg)
+** @param [r] lon [double] longitude (deg)
+**
+** @return [int32] success
+************************************************************************/
+
+int32 GPS_Rqst_Send_Position(int32 fd, double lat, double lon)
+{
+ int32 ret=0;
+
+ switch(gps_position_transfer)
+ {
+ case pA700:
+ ret = GPS_A700_Rqst(fd, lat, lon);
+ break;
+ default:
+ GPS_Error("Rqst_Send_Position: Unknown position protocol");
+ return PROTOCOL_ERROR;
+ }
+
+ return ret;
+}
+
+
+
+/* @funcstatic GPS_A700_Rqst *******************************************
+**
+** Send position to GPS
+**
+** @param [r] fd [int32] file descriptor
+** @param [r] lat [double] latitude (deg)
+** @param [r] lon [double] longitute (deg)
+**
+** @return [int32] success
+************************************************************************/
+static int32 GPS_A700_Rqst(int32 fd, double lat, double lon)
+{
+ GPS_PPacket tra;
+ GPS_PPacket rec;
+
+ if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New()))
+ return MEMORY_ERROR;
+
+
+ switch(gps_position_type)
+ {
+ case pD700:
+ GPS_D700_Send(&tra,lat,lon);
+ break;
+ default:
+ GPS_Error("A700_Rqst: Unknown position protocol");
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+ return PROTOCOL_ERROR;
+ }
+
+ if(!GPS_Write_Packet(fd,tra))
+ return gps_errno;
+
+ if(!GPS_Get_Ack(fd, &tra, &rec))
+ return gps_errno;
+
+
+ GPS_Packet_Del(&tra);
+ GPS_Packet_Del(&rec);
+
+ return 1;
+}
+
--- /dev/null
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef gpsrqst_h
+#define gpsrqst_h
+
+
+#include "gps.h"
+
+int32 GPS_Rqst_Send_Time(int32 fd, time_t Time);
+int32 GPS_Rqst_Send_Position(int32 fd, double lat, double lon);
+
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null
+/********************************************************************
+** @source JEEPS packet construction, sending and ack functions
+**
+** @author Copyright (C) 1999 Alan Bleasby
+** @version 1.0
+** @modified Dec 28 1999 Alan Bleasby. First version
+** @@
+**
+** This library is free software; you can redistribute it and/or
+** modify it under the terms of the GNU Library General Public
+** License as published by the Free Software Foundation; either
+** version 2 of the License, or (at your option) any later version.
+**
+** This library is distributed in the hope that it will be useful,
+** but WITHOUT ANY WARRANTY; without even the implied warranty of
+** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+** Library General Public License for more details.
+**
+** You should have received a copy of the GNU Library General Public
+** License along with this library; if not, write to the
+** Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+** Boston, MA 02111-1307, USA.
+********************************************************************/
+#include "gps.h"
+#include <stdio.h>
+#include <unistd.h>
+#include <errno.h>
+
+
+/* @func GPS_Make_Packet ***********************************************
+**
+** Forms a complete packet to send
+**
+** @param [w] packet [GPS_PPacket *] packet string
+** @param [r] type [UC] packet type
+** @param [r] data [UC *] data string
+** @param [r] n [int16] number of bytes in data string
+**
+** @return [void]
+************************************************************************/
+
+void GPS_Make_Packet(GPS_PPacket *packet, UC type, UC *data, int16 n)
+{
+ UC *p;
+ UC *q;
+
+ int32 i;
+ UC chk=0;
+
+ p = data;
+ q = (*packet)->data;
+
+ (*packet)->dle = DLE;
+ (*packet)->edle = DLE;
+ (*packet)->etx = ETX;
+ (*packet)->n = n;
+ (*packet)->type = type;
+ (*packet)->bytes = 0;
+
+ chk -= type;
+
+ if(n == DLE)
+ {
+ ++(*packet)->bytes;
+ *q++ = DLE;
+ }
+
+
+ chk -= (UC) n;
+
+ for(i=0;i<n;++i)
+ {
+ if(*p == DLE)
+ {
+ ++(*packet)->bytes;
+ *q++ = DLE;
+ }
+ chk -= *p;
+ *q++ = *p++;
+ ++(*packet)->bytes;
+ }
+
+ if(chk == DLE)
+ {
+ *q++ = DLE;
+ ++(*packet)->bytes;
+ }
+
+ (*packet)->chk = chk;
+
+ return;
+}
+
+
+
+
+/* @func GPS_Write_Packet ***********************************************
+**
+** Forms a complete packet to send
+**
+** @param [w] fd [int32] file descriptor
+** @param [r] packet [GPS_PPacket] packet
+**
+** @return [int32] number of bytes in the packet
+************************************************************************/
+
+int32 GPS_Write_Packet(int32 fd, GPS_PPacket packet)
+{
+ size_t ret;
+
+ if((ret=GPS_Serial_Write(fd,(const void *)&packet->dle,(size_t)3)) == -1)
+ {
+ perror("write");
+ GPS_Error("SEND: Write to GPS failed");
+ return 0;
+ }
+ if(ret!=3)
+ {
+ GPS_Error("SEND: Incomplete write to GPS");
+ return 0;
+ }
+
+ if((ret=GPS_Serial_Write(fd,(const void *)packet->data,(size_t)packet->bytes)) == -1)
+ {
+ perror("write");
+ GPS_Error("SEND: Write to GPS failed");
+ return 0;
+ }
+ if(ret!=packet->bytes)
+ {
+ GPS_Error("SEND: Incomplete write to GPS");
+ return 0;
+ }
+
+
+ if((ret=GPS_Serial_Write(fd,(const void *)&packet->chk,(size_t)3)) == -1)
+ {
+ perror("write");
+ GPS_Error("SEND: Write to GPS failed");
+ return 0;
+ }
+ if(ret!=3)
+ {
+ GPS_Error("SEND: Incomplete write to GPS");
+ return 0;
+ }
+
+
+ return 1;
+}
+
+
+/* @func GPS_Send_Ack ***********************************************
+**
+** Send an acknowledge packet
+**
+** @param [w] fd [int32] file descriptor
+** @param [r] tra [GPS_PPacket *] packet to transmit
+** @param [r] rec [GPS_PPacket *] last packet received
+**
+** @return [int32] success
+************************************************************************/
+
+int32 GPS_Send_Ack(int32 fd, GPS_PPacket *tra, GPS_PPacket *rec)
+{
+ UC data[2];
+
+ GPS_Util_Put_Short(data,(US)(*rec)->type);
+ GPS_Make_Packet(tra,LINK_ID[0].Pid_Ack_Byte,data,2);
+ if(!GPS_Write_Packet(fd,*tra))
+ {
+ GPS_Error("Error acknowledging packet");
+ gps_errno = SERIAL_ERROR;
+ return 0;
+ }
+
+ return 1;
+}
--- /dev/null
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef gpssend_h
+#define gpssend_h
+
+
+#include "gps.h"
+
+#define GPS_ARB_LEN 1024
+
+UC gps_sendbuf[GPS_ARB_LEN];
+
+void GPS_Make_Packet(GPS_PPacket *packet, UC type, UC *data, int16 n);
+int32 GPS_Write_Packet(int32 fd, GPS_PPacket packet);
+int32 GPS_Send_Ack(int32 fd, GPS_PPacket *tra, GPS_PPacket *rec);
+
+
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null
+/********************************************************************
+** @source JEEPS serial port low level functions
+**
+** @author Copyright (C) 1999,2000 Alan Bleasby
+** @version 1.0
+** @modified December 28th 1999 Alan Bleasby. First version
+** @modified June 29th 2000 Alan Bleasby. NMEA additions
+** @@
+**
+** This library is free software; you can redistribute it and/or
+** modify it under the terms of the GNU Library General Public
+** License as published by the Free Software Foundation; either
+** version 2 of the License, or (at your option) any later version.
+**
+** This library is distributed in the hope that it will be useful,
+** but WITHOUT ANY WARRANTY; without even the implied warranty of
+** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+** Library General Public License for more details.
+**
+** You should have received a copy of the GNU Library General Public
+** License along with this library; if not, write to the
+** Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+** Boston, MA 02111-1307, USA.
+********************************************************************/
+#include "gps.h"
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <time.h>
+#include <sys/time.h>
+
+
+#if __WIN32__
+
+#include <windows.h>
+/*
+ * Rather than teaching the rest of this code about Windows serial APIs
+ * we'll weenie out, violate good layering, and just keep our handle
+ * internal. This means we ignore that 'fd' number that gets passed in.
+ */
+
+static HANDLE comport;
+
+int32 GPS_Serial_On(const char *port, int32 *fd)
+{
+ DCB tio;
+ COMMTIMEOUTS timeout;
+
+ comport = CreateFile(port, GENERIC_READ|GENERIC_WRITE, 0, NULL,
+ OPEN_EXISTING, 0, NULL);
+ if (comport == INVALID_HANDLE_VALUE) {
+ gps_errno = SERIAL_ERROR;
+ return 0;
+ }
+
+ tio.DCBlength = sizeof(DCB);
+ GetCommState (comport, &tio);
+ tio.BaudRate = CBR_9600;
+ tio.fBinary = TRUE;
+ tio.fParity = TRUE;
+ tio.fOutxCtsFlow = FALSE;
+ tio.fOutxDsrFlow = FALSE;
+ tio.fDtrControl = DTR_CONTROL_ENABLE;
+ tio.fDsrSensitivity = FALSE;
+ tio.fTXContinueOnXoff = TRUE;
+ tio.fOutX = FALSE;
+ tio.fInX = FALSE;
+ tio.fErrorChar = FALSE;
+ tio.fNull = FALSE;
+ tio.fRtsControl = RTS_CONTROL_ENABLE;
+ tio.fAbortOnError = FALSE;
+ tio.ByteSize = 8;
+ tio.Parity = NOPARITY;
+ tio.StopBits = ONESTOPBIT;
+
+ if (!SetCommState (comport, &tio)) {
+ CloseHandle(comport);
+ gps_errno = SERIAL_ERROR;
+ return 0;
+ }
+
+ GetCommTimeouts (comport, &timeout);
+ timeout.ReadIntervalTimeout = 10;
+ timeout.WriteTotalTimeoutMultiplier = 10;
+ timeout.WriteTotalTimeoutConstant = 1000;
+ if (!SetCommTimeouts (comport, &timeout)) {
+ CloseHandle (comport);
+ gps_errno = SERIAL_ERROR;
+ return 0;
+ }
+ *fd = 1;
+ return 1;
+}
+
+int32 GPS_Serial_Off(const char *port, int32 fd)
+{
+ CloseHandle(comport);
+ return 1;
+}
+
+int32 GPS_Serial_Chars_Ready(int32 fd)
+{
+ return 1;
+}
+
+int32 GPS_Serial_Wait(int32 fd)
+{
+ return 1;
+}
+
+int32 GPS_Serial_Flush(int32 fd)
+{
+ return 1;
+}
+
+int32 GPS_Serial_Write(int ignored, void *obuf, int size)
+{
+ DWORD len;
+ WriteFile (comport, obuf, size, &len, NULL);
+ if (len != size) {
+ fatal ("Write error. Wrote %d of %d bytes.", len, size);
+ }
+ return len;
+}
+
+int GPS_Serial_Read(int ignored, void *ibuf, int size)
+{
+ DWORD cnt;
+
+ ReadFile(comport, ibuf, size, &cnt, NULL);
+ return cnt;
+}
+
+#else
+
+#include <sys/ioctl.h>
+#include <termios.h>
+
+static struct termios gps_ttysave;
+
+/* @func GPS_Serial_Restoretty ***********************************************
+**
+** Save tty information for the serial post to be used
+**
+** @param [r] port [const char *] port e.g. ttyS1
+**
+** @return [int32] false upon error
+************************************************************************/
+
+int32 GPS_Serial_Savetty(const char *port)
+{
+ int32 fd;
+
+ if((fd = open(port, O_RDWR|O_NDELAY))==-1)
+ {
+ perror("open");
+ gps_errno = SERIAL_ERROR;
+ GPS_Error("SERIAL: Cannot open serial port");
+ return 0;
+ }
+
+ if(tcgetattr(fd,&gps_ttysave)==-1)
+ {
+ perror("tcgetattr");
+ gps_errno = HARDWARE_ERROR;
+ GPS_Error("SERIAL: tcgetattr error");
+ return 0;
+ }
+
+
+ if(!GPS_Serial_Close(fd,port))
+ {
+ gps_errno = SERIAL_ERROR;
+ GPS_Error("GPS_Serial_Savetty: Close error");
+ return 0;
+ }
+
+ return 1;
+}
+
+
+/* @func GPS_Serial_Restoretty ***********************************************
+**
+** Restore serial post to condition before AJBGPS called
+**
+** @param [r] port [const char *] port e.g. ttyS1
+**
+** @return [int32] false upon error
+************************************************************************/
+
+int32 GPS_Serial_Restoretty(const char *port)
+{
+ int32 fd;
+
+ if((fd = open(port, O_RDWR|O_NDELAY))==-1)
+ {
+ perror("open");
+ gps_errno = HARDWARE_ERROR;
+ GPS_Error("SERIAL: Cannot open serial port");
+ return 0;
+ }
+
+ if(tcsetattr(fd, TCSAFLUSH, &gps_ttysave)==-1)
+ {
+ perror("ioctl");
+ gps_errno = HARDWARE_ERROR;
+ GPS_Error("SERIAL: tcsetattr error");
+ return 0;
+ }
+
+ return 1;
+}
+
+
+
+/* @func GPS_Serial_Open ***********************************************
+**
+** Open a serial port 8bits 1 stop bit 9600 baud
+**
+** @param [w] fd [int32 *] file descriptor
+** @param [r] port [const char *] port e.g. ttyS1
+**
+** @return [int32] false upon error
+************************************************************************/
+
+int32 GPS_Serial_Open(int32 *fd, const char *port)
+{
+ struct termios tty;
+
+
+ if((*fd = open(port, O_RDWR | O_NDELAY | O_NOCTTY))==-1)
+ {
+ perror("open");
+ GPS_Error("SERIAL: Cannot open serial port");
+ gps_errno = SERIAL_ERROR;
+ return 0;
+ }
+
+
+ if(tcgetattr(*fd,&tty)==-1)
+ {
+ perror("tcgetattr");
+ GPS_Error("SERIAL: tcgetattr error");
+ gps_errno = SERIAL_ERROR;
+ return 0;
+ }
+
+ tty.c_cflag |= (CREAD | CS8 | CSIZE | CLOCAL);
+ cfsetospeed(&tty,B9600);
+ cfsetispeed(&tty,B9600);
+
+ tty.c_lflag &= 0x0;
+ tty.c_iflag &= 0x0;
+ tty.c_oflag &= 0x0;
+
+
+ if(tcsetattr(*fd,TCSANOW,&tty)==-1)
+ {
+ perror("tcsetattr");
+ GPS_Error("SERIAL: tcsetattr error");
+ return 0;
+ }
+
+ return 1;
+}
+
+int32 GPS_Serial_Read(int32 handle, void *ibuf, int size)
+{
+ return read(handle, ibuf, size);
+}
+
+int32 GPS_Serial_Write(int32 handle, void *obuf, int size)
+{
+ return write(handle, obuf, size);
+}
+
+
+/* @func GPS_Serial_Flush ***********************************************
+**
+** Flush the serial lines
+**
+** @param [w] fd [int32] file descriptor
+**
+** @return [int32] false upon error
+************************************************************************/
+int32 GPS_Serial_Flush(int32 fd)
+{
+
+ if(tcflush(fd,TCIOFLUSH))
+ {
+ perror("tcflush");
+ GPS_Error("SERIAL: tcflush error");
+ gps_errno = SERIAL_ERROR;
+ return 0;
+ }
+
+ return 1;
+}
+
+
+
+/* @func GPS_Serial_Close ***********************************************
+**
+** Close serial port
+**
+** @param [r] fd [int32 ] file descriptor
+** @param [r] port [const char *] port e.g. ttyS1
+**
+** @return [int32] false upon error
+************************************************************************/
+
+int32 GPS_Serial_Close(int32 fd, const char *port)
+{
+ if(close(fd)==-1)
+ {
+ perror("close");
+ GPS_Error("SERIAL: Error closing serial port");
+ gps_errno = SERIAL_ERROR;
+ return 0;
+ }
+
+ return 1;
+}
+
+
+/* @func GPS_Serial_Chars_Ready *****************************************
+**
+** Query port to see if characters are waiting to be read
+**
+** @param [r] fd [int32 ] file descriptor
+**
+** @return [int32] true if chars waiting
+************************************************************************/
+
+int32 GPS_Serial_Chars_Ready(int32 fd)
+{
+ fd_set rec;
+ struct timeval t;
+
+ FD_ZERO(&rec);
+ FD_SET(fd,&rec);
+
+ t.tv_sec = 0;
+ t.tv_usec = 0;
+
+ (void) select(fd+1,&rec,NULL,NULL,&t);
+ if(FD_ISSET(fd,&rec))
+ return 1;
+
+ return 0;
+}
+
+
+
+/* @func GPS_Serial_Wait ***********************************************
+**
+** Wait 80 milliseconds before testing for input. The GPS delay
+** appears to be around 40-50 milliseconds. Doubling the value is to
+** allow some leeway.
+**
+** @param [r] fd [int32 ] file descriptor
+**
+** @return [int32] true if serial chars waiting
+************************************************************************/
+
+int32 GPS_Serial_Wait(int32 fd)
+{
+ fd_set rec;
+ struct timeval t;
+
+ FD_ZERO(&rec);
+ FD_SET(fd,&rec);
+
+ t.tv_sec = 0;
+ t.tv_usec = usecDELAY;
+
+ (void) select(fd+1,&rec,NULL,NULL,&t);
+ if(FD_ISSET(fd,&rec))
+ return 1;
+
+ return 0;
+}
+
+
+
+/* @func GPS_Serial_On *****************************************
+**
+** Set up port
+**
+** @param [r] port [const char *] port
+** @param [w] fd [int32 *] file descriptor
+**
+** @return [int32] success
+************************************************************************/
+
+int32 GPS_Serial_On(const char *port, int32 *fd)
+{
+
+ if(!GPS_Serial_Savetty(port))
+ {
+ GPS_Error("Cannot access serial port");
+ gps_errno = SERIAL_ERROR;
+ return 0;
+ }
+
+ if(!GPS_Serial_Open(fd,port))
+ {
+ GPS_Error("Cannot open serial port");
+ gps_errno = SERIAL_ERROR;
+ return 0;
+ }
+
+ return 1;
+}
+
+
+
+/* @func GPS_Serial_Off ***********************************************
+**
+** Done with port
+**
+** @param [r] port [const char *] port
+** @param [r] fd [int32 ] file descriptor
+**
+** @return [int32] success
+************************************************************************/
+
+int32 GPS_Serial_Off(const char *port, int32 fd)
+{
+ if(!GPS_Serial_Close(fd,port))
+ {
+ GPS_Error("Error Closing port");
+ gps_errno = HARDWARE_ERROR;
+ return 0;
+ }
+
+ if(!GPS_Serial_Restoretty(port))
+ {
+ GPS_Error("Error restoring port");
+ gps_errno = HARDWARE_ERROR;
+ return 0;
+ }
+
+ return 1;
+}
+
+
+
+
+
+
+
+/* @func GPS_Serial_Open_NMEA ******************************************
+**
+** Open a serial port 8bits 1 stop bit 4800 baud
+**
+** @param [w] fd [int32 *] file descriptor
+** @param [r] port [const char *] port e.g. ttyS1
+**
+** @return [int32] false upon error
+************************************************************************/
+
+int32 GPS_Serial_Open_NMEA(int32 *fd, const char *port)
+{
+ struct termios tty;
+
+
+ if((*fd = open(port, O_RDWR | O_NDELAY | O_NOCTTY))==-1)
+ {
+ perror("open");
+ GPS_Error("SERIAL: Cannot open serial port");
+ gps_errno = SERIAL_ERROR;
+ return 0;
+ }
+
+
+ if(tcgetattr(*fd,&tty)==-1)
+ {
+ perror("tcgetattr");
+ GPS_Error("SERIAL: tcgetattr error");
+ gps_errno = SERIAL_ERROR;
+ return 0;
+ }
+
+
+ tty.c_cflag |= (CREAD | CS8 | CSIZE | CLOCAL);
+ cfsetospeed(&tty,B4800);
+ cfsetispeed(&tty,B4800);
+
+ tty.c_lflag &= 0x0;
+ tty.c_iflag &= 0x0;
+ tty.c_oflag &= 0x0;
+
+
+ if(tcsetattr(*fd,TCSANOW,&tty)==-1)
+ {
+ perror("tcsetattr");
+ GPS_Error("SERIAL: tcsetattr error");
+ return 0;
+ }
+
+ return 1;
+}
+
+
+
+
+
+
+
+/* @func GPS_Serial_On_NMEA ********************************************
+**
+** Set up port for NMEA
+**
+** @param [r] port [const char *] port
+** @param [w] fd [int32 *] file descriptor
+**
+** @return [int32] success
+************************************************************************/
+int32 GPS_Serial_On_NMEA(const char *port, int32 *fd)
+{
+
+ if(!GPS_Serial_Savetty(port))
+ {
+ GPS_Error("Cannot access serial port");
+ gps_errno = SERIAL_ERROR;
+ return 0;
+ }
+
+ if(!GPS_Serial_Open_NMEA(fd,port))
+ {
+ GPS_Error("Cannot open serial port");
+ gps_errno = SERIAL_ERROR;
+ return 0;
+ }
+
+ return 1;
+}
+#endif /* __WIN32__ */
--- /dev/null
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef gpsserial_h
+#define gpsserial_h
+
+
+#include "gps.h"
+
+#define usecDELAY 180000 /* Microseconds before GPS sends A001 */
+
+int32 GPS_Serial_Chars_Ready(int32 fd);
+int32 GPS_Serial_Close(int32 fd, const char *port);
+int32 GPS_Serial_Open(int32 *fd, const char *port);
+int32 GPS_Serial_Open_NMEA(int32 *fd, const char *port);
+int32 GPS_Serial_Restoretty(const char *port);
+int32 GPS_Serial_Savetty(const char *port);
+int32 GPS_Serial_On(const char *port, int32 *fd);
+int32 GPS_Serial_Off(const char *port, int32 fd);
+int32 GPS_Serial_Wait(int32 fd);
+int32 GPS_Serial_Flush(int32 fd);
+int32 GPS_Serial_On_NMEA(const char *port, int32 *fd);
+
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null
+/********************************************************************
+** @source JEEPS utility functions
+**
+** @author Copyright (C) 1999 Alan Bleasby
+** @version 1.0
+** @modified Dec 28 1999 Alan Bleasby. First version
+** @@
+**
+** This library is free software; you can redistribute it and/or
+** modify it under the terms of the GNU Library General Public
+** License as published by the Free Software Foundation; either
+** version 2 of the License, or (at your option) any later version.
+**
+** This library is distributed in the hope that it will be useful,
+** but WITHOUT ANY WARRANTY; without even the implied warranty of
+** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+** Library General Public License for more details.
+**
+** You should have received a copy of the GNU Library General Public
+** License along with this library; if not, write to the
+** Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+** Boston, MA 02111-1307, USA.
+********************************************************************/
+#include "gps.h"
+// #include <termios.h>
+#include <fcntl.h>
+
+static int32 gps_endian_called=0;
+static int32 GPS_Little=0;
+
+int32 gps_warning = 0;
+int32 gps_error = 0;
+int32 gps_user = 0;
+int32 gps_show_bytes = 0;
+int32 gps_errno = 0;
+
+
+/* @func GPS_Util_Little ***********************************************
+**
+** Determine endian nature of host
+**
+** @return [int32] true if little-endian
+************************************************************************/
+
+int32 GPS_Util_Little(void)
+{
+ static union lb
+ {
+ char chars[sizeof(int32)];
+ int32 i;
+ }
+ data;
+
+ if(!gps_endian_called)
+ {
+ gps_endian_called = 1;
+ data.i = 0;
+ *data.chars = '\1';
+ if(data.i == 1)
+ GPS_Little = 1;
+ else
+ GPS_Little = 0;
+ }
+
+ return GPS_Little;
+}
+
+
+/* @func GPS_Util_Get_Short ********************************************
+**
+** Get a short from a string
+**
+** @return [US] value
+************************************************************************/
+
+US GPS_Util_Get_Short(const UC *s)
+{
+ static US ret;
+ UC *p;
+
+ p = (UC *)&ret;
+
+ if(!GPS_Little)
+ {
+ *p++ = *(s+1);
+ *p = *s;
+ }
+ else
+ {
+ *p++ = *s;
+ *p = *(s+1);
+ }
+
+ return ret;
+}
+
+
+
+/* @func GPS_Util_Put_Short ********************************************
+**
+** Put a short to a string
+**
+** @param [w] s [UC *] string to write to
+** @param [r] v [const US] short to write
+**
+** @return [void]
+************************************************************************/
+
+void GPS_Util_Put_Short(UC *s, const US v)
+{
+ UC *p;
+
+ p = (UC *)&v;
+
+ if(!GPS_Little)
+ {
+ *s++ = *(p+1);
+ *s = *p;
+ }
+ else
+ {
+ *s++ = *p;
+ *s = *(p+1);
+ }
+
+ return;
+}
+
+
+
+/* @func GPS_Util_Get_Double ********************************************
+**
+** Get a double from a string
+**
+** @return [double] value
+************************************************************************/
+
+double GPS_Util_Get_Double(const UC *s)
+{
+ double ret;
+ UC *p;
+ int32 i;
+
+ p = (UC *)&ret;
+
+
+ if(!GPS_Little)
+ for(i=sizeof(double)-1;i>-1;--i)
+ *p++ = s[i];
+ else
+ for(i=0;i<sizeof(double);++i)
+ *p++ = s[i];
+
+ return ret;
+}
+
+
+
+/* @func GPS_Util_Put_Double ********************************************
+**
+** Put a double to a string
+**
+** @param [w] s [UC *] string to write to
+** @param [r] v [const double] double to write
+**
+** @return [void]
+************************************************************************/
+
+void GPS_Util_Put_Double(UC *s, const double v)
+{
+ UC *p;
+ int32 i;
+
+ p = (UC *)&v;
+
+ if(!GPS_Little)
+ for(i=sizeof(double)-1;i>-1;--i)
+ s[i] = *p++;
+ else
+ for(i=0;i<sizeof(double);++i)
+ s[i] = *p++;
+
+ return;
+}
+
+
+
+
+/* @func GPS_Util_Get_Int ********************************************
+**
+** Get an int from a string
+**
+** @return [int32] value
+************************************************************************/
+
+int32 GPS_Util_Get_Int(const UC *s)
+{
+ int32 ret;
+ UC *p;
+ int32 i;
+
+ p = (UC *)&ret;
+
+
+ if(!GPS_Little)
+ for(i=sizeof(int32)-1;i>-1;--i)
+ *p++ = s[i];
+ else
+ for(i=0;i<sizeof(int32);++i)
+ *p++ = s[i];
+
+ return ret;
+}
+
+
+
+/* @func GPS_Util_Put_Int ********************************************
+**
+** Put a int to a string
+**
+** @param [w] s [UC *] string to write to
+** @param [r] v [const int32] int to write
+**
+** @return [void]
+************************************************************************/
+
+void GPS_Util_Put_Int(UC *s, const int32 v)
+{
+ UC *p;
+ int32 i;
+
+ p = (UC *)&v;
+
+ if(!GPS_Little)
+ for(i=sizeof(int32)-1;i>-1;--i)
+ s[i] = *p++;
+ else
+ for(i=0;i<sizeof(int32);++i)
+ s[i] = *p++;
+
+ return;
+}
+
+
+
+/* @func GPS_Util_Get_Uint ********************************************
+**
+** Get an unsigned int from a string
+**
+** @return [uint32] value
+************************************************************************/
+
+uint32 GPS_Util_Get_Uint(const UC *s)
+{
+ uint32 ret;
+ UC *p;
+ int32 i;
+
+ p = (UC *)&ret;
+
+
+ if(!GPS_Little)
+ for(i=sizeof(uint32)-1;i>-1;--i)
+ *p++ = s[i];
+ else
+ for(i=0;i<sizeof(uint32);++i)
+ *p++ = s[i];
+
+ return ret;
+}
+
+
+
+/* @func GPS_Util_Put_Uint ********************************************
+**
+** Put an unisgned int to a string
+**
+** @param [w] s [UC *] string to write to
+** @param [r] v [const uint32] unsigned int to write
+**
+** @return [void]
+************************************************************************/
+
+void GPS_Util_Put_Uint(UC *s, const uint32 v)
+{
+ UC *p;
+ int32 i;
+
+ p = (UC *)&v;
+
+ if(!GPS_Little)
+ for(i=sizeof(uint32)-1;i>-1;--i)
+ s[i] = *p++;
+ else
+ for(i=0;i<sizeof(uint32);++i)
+ s[i] = *p++;
+
+ return;
+}
+
+
+
+/* @func GPS_Util_Get_Float ********************************************
+**
+** Get a float from a string
+**
+** @return [float] value
+************************************************************************/
+
+float GPS_Util_Get_Float(const UC *s)
+{
+ float ret;
+ UC *p;
+ int32 i;
+
+ p = (UC *)&ret;
+
+
+ if(!GPS_Little)
+ for(i=sizeof(float)-1;i>-1;--i)
+ *p++ = s[i];
+ else
+ for(i=0;i<sizeof(float);++i)
+ *p++ = s[i];
+
+ return ret;
+}
+
+
+
+/* @func GPS_Util_Put_Float ********************************************
+**
+** Put a float to a string
+**
+** @param [w] s [UC *] string to write to
+** @param [r] v [const float] float to write
+**
+** @return [void]
+************************************************************************/
+
+void GPS_Util_Put_Float(UC *s, const float v)
+{
+ UC *p;
+ int32 i;
+
+ p = (UC *)&v;
+
+ if(!GPS_Little)
+ for(i=sizeof(float)-1;i>-1;--i)
+ s[i] = *p++;
+ else
+ for(i=0;i<sizeof(float);++i)
+ s[i] = *p++;
+
+ return;
+}
+
+
+#if 0
+/* @func GPS_Util_Canon ****************************************************
+**
+** Sets or unsets canonical mode
+** NB: Must have called this with True before calling with False
+** NB: Remember to trun it off (false) eventually
+**
+** @param [r] state [int32] state=true->raw state=false->normal
+** @return [void]
+** @@
+****************************************************************************/
+
+void GPS_Util_Canon(int32 state)
+{
+ static struct termios tty;
+ static struct termios sv;
+
+
+ if(state)
+ {
+ tcgetattr(1,&sv);
+ tcgetattr(1, &tty);
+ tty.c_cc[VMIN]='\1';
+ tty.c_cc[VTIME]='\0';
+ tcsetattr(1,TCSANOW,&tty);
+ tty.c_lflag &= ~(ICANON | ECHO);
+ tcsetattr(1, TCSANOW, &tty);
+ }
+ else
+ tcsetattr(1, TCSANOW, &sv);
+
+ return;
+}
+#endif
+
+#if 0
+/* @func GPS_Util_Block ****************************************************
+**
+** Sets or unsets blocking
+** @modified 13-01-2000 to return an int
+**
+** @param [r] fd [int32] file descriptor
+** @param [r] state [int32] state=true->block state=false->non-block
+**
+** @return [int32] success
+** @@
+****************************************************************************/
+
+int32 GPS_Util_Block(int32 fd, int32 state)
+{
+ static int32 notcalled=1;
+ static int32 block;
+ static int32 noblock;
+ int32 f;
+
+ gps_errno = HARDWARE_ERROR;
+
+ if(notcalled)
+ {
+ notcalled = 0;
+ if((f=fcntl(fd,F_GETFL,0))==-1)
+ {
+ GPS_Error("Util_Block: FCNTL error");
+ return 0;
+ }
+ block = f & ~O_NDELAY;
+ noblock = f | O_NDELAY;
+ }
+
+ if(state)
+ {
+ if(fcntl(fd,F_SETFL,block)==-1)
+ {
+ GPS_Error("Util_Block: Error blocking");
+ return 0;
+ }
+ }
+ else
+ {
+ if(fcntl(fd,F_SETFL,noblock)==-1)
+ {
+ GPS_Error("Util_Block: Error unblocking");
+ return 0;
+ }
+ }
+
+ return 1;
+}
+#endif
+
+
+/* @func GPS_Warning ********************************************************
+**
+** Prints warning if gps_warning is true
+**
+** @param [r] s [char *] warning
+**
+** @return [void]
+** @@
+****************************************************************************/
+
+void GPS_Warning(char *s)
+{
+ if(!gps_warning)
+ return;
+
+ fprintf(stderr,"[WARNING] %s\n",s);
+ fflush(stderr);
+
+ return;
+}
+
+
+/* @func GPS_Fatal ********************************************************
+**
+** Always prints error and exits program
+** Bad thing for a library so the library doesn't call it.
+**
+** @param [r] s [char *] fatal error
+**
+** @return [void]
+** @@
+****************************************************************************/
+
+void GPS_Fatal(char *s)
+{
+
+ fprintf(stderr,"[FATAL] %s\n",s);
+ exit(0);
+ return;
+}
+
+
+
+/* @func GPS_Error **********************************************************
+**
+** Prints Error if gps_error is true
+**
+** @param [r] s [char *] error
+**
+** @return [void]
+** @@
+****************************************************************************/
+
+void GPS_Error(char *s)
+{
+ if(!gps_error)
+ return;
+
+ fprintf(stderr,"[ERROR] %s\n",s);
+ fflush(stderr);
+
+ return;
+}
+
+
+/* @func GPS_Enable_Error ***************************************************
+**
+** Enable error message printing
+**
+** @return [void]
+** @@
+****************************************************************************/
+
+void GPS_Enable_Error(void)
+{
+ gps_error = 1;
+ return;
+}
+
+
+
+/* @func GPS_Enable_Warning ***************************************************
+**
+** Enable warning message printing
+**
+** @return [void]
+** @@
+****************************************************************************/
+
+void GPS_Enable_Warning(void)
+{
+ gps_warning = 1;
+ return;
+}
+
+
+
+/* @func GPS_Disable_Error ***************************************************
+**
+** Disable error message printing
+**
+** @return [void]
+** @@
+****************************************************************************/
+
+void GPS_Disable_Error(void)
+{
+ gps_error = 0;
+ return;
+}
+
+
+
+/* @func GPS_Disable_Warning ***********************************************
+**
+** Disable warning message printing
+**
+** @return [void]
+** @@
+****************************************************************************/
+
+void GPS_Disable_Warning(void)
+{
+ gps_warning = 0;
+ return;
+}
+
+
+
+/* @func GPS_User ********************************************************
+**
+** Prints a message if gps_user is true
+**
+** @param [r] s [char *] message
+**
+** @return [void]
+** @@
+****************************************************************************/
+
+void GPS_User(char *s)
+{
+ if(!gps_user)
+ return;
+
+ fprintf(stdout,"%s\n",s);
+ fflush(stdout);
+
+ return;
+}
+
+/* @func GPS_Disable_User ***********************************************
+**
+** Disable message printing
+**
+** @return [void]
+** @@
+****************************************************************************/
+
+void GPS_Disable_User(void)
+{
+ gps_user = 0;
+ return;
+}
+
+
+/* @func GPS_Enable_User ***********************************************
+**
+** Disable warning message printing
+**
+** @return [void]
+** @@
+****************************************************************************/
+
+void GPS_Enable_User(void)
+{
+ gps_user = 1;
+ return;
+}
+
+
+/* @func GPS_Diagnose ********************************************************
+**
+** Prints bytes read from gps if gps_show_bytes is set
+**
+** @param [r] cs [int32] byte read
+**
+** @return [void]
+** @@
+****************************************************************************/
+
+void GPS_Diagnose(int32 c)
+{
+ if(!gps_show_bytes)
+ return;
+
+ fprintf(stdout,"%d\n",(int)c);
+ fflush(stdout);
+
+ return;
+}
+
+
+
+/* @func GPS_Enable_Diagnose ***********************************************
+**
+** Enable diagnosis mode
+**
+** @return [void]
+** @@
+****************************************************************************/
+
+void GPS_Enable_Diagnose(void)
+{
+ gps_show_bytes = 1;
+ return;
+}
+
+
+
+/* @func GPS_Disble_Diagnose ***********************************************
+**
+** Disable diagnosis mode
+**
+** @return [void]
+** @@
+****************************************************************************/
+
+void GPS_Disable_Diagnose(void)
+{
+ gps_show_bytes = 0;
+ return;
+}
--- /dev/null
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#ifndef gpsutil_h
+#define gpsutil_h
+
+
+#include "gps.h"
+
+int32 GPS_Util_Little(void);
+
+US GPS_Util_Get_Short(const UC *s);
+void GPS_Util_Put_Short(UC *s, const US v);
+int32 GPS_Util_Get_Int(const UC *s);
+void GPS_Util_Put_Int(UC *s, const int32 v);
+double GPS_Util_Get_Double(const UC *s);
+void GPS_Util_Put_Double(UC *s, const double v);
+float GPS_Util_Get_Float(const UC *s);
+void GPS_Util_Put_Float(UC *s, const float v);
+void GPS_Util_Canon(int32 state);
+int32 GPS_Util_Block(int32 fd, int32 state);
+void GPS_Util_Put_Uint(UC *s, const uint32 v);
+uint32 GPS_Util_Get_Uint(const UC *s);
+
+void GPS_Warning(char *s);
+void GPS_Error(char *s);
+void GPS_Fatal(char *s);
+void GPS_Enable_Error(void);
+void GPS_Enable_Warning(void);
+void GPS_Disable_Error(void);
+void GPS_Disable_Warning(void);
+void GPS_User(char *s);
+void GPS_Disable_User(void);
+void GPS_Enable_User(void);
+void GPS_Diagnose(int32 c);
+void GPS_Enable_Diagnose(void);
+void GPS_Disable_Diagnose(void);
+
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null
+#include "gps.h"
+// #include "jeeps.h"
+
+main()
+{
+ int n;
+ GPS_PWay *way;
+ GPS_PWay *array;
+
+ if (GPS_Init("/dev/ttyS0") < 0) {
+ fprintf(stderr, "Can't init\n");
+ }
+
+ if((n=GPS_Command_Get_Waypoint("/dev/ttyS0", &way))<0) {
+ fprintf(stderr, "can't get\n");
+ return;
+ }
+// fprintf(stdout," Done\n");
+
+ GPS_Fmt_Print_Waypoint(way, n, stdout);
+
+ array = (GPS_PWay *) calloc(1, sizeof(GPS_PWay));
+ array[0] = GPS_Way_New();
+ strcpy(array[0]->ident,"lower @#$%^&* rocks");
+ strcpy(array[0]->cmnt,"COMMENTCOMMENTCOMMENTCOMMENTCOMMENT");
+ array[0]->wpt_class = 0;
+ array[0]->lat = 1.234;
+ array[0]->lon = 1.234;
+GPS_Command_Send_Waypoint("/dev/ttyS0", array, 1);
+
+}
-CC=/home/robertl/cross-tools/bin/i386-mingw32msvc-gcc -Iinclude -I../coldsync -DCETUS
+CC=/home/robertl/cross-tools/bin/i386-mingw32msvc-gcc -Iinclude
VPATH=..
gpsbabel.exe:
static const char *badchars = DEFAULT_BADCHARS;
static int mustupper = 0;
+static int whitespaceok = 1;
static const char needmem[] =
"mkshort: could not reallocate memory for string\n";
}
}
+void
+setshort_whitespace_ok(int l)
+{
+ whitespaceok = l;
+}
+
/*
* Externally callable function to set the string of characters
* that must never appear in a string returned by mkshort. NULL
/*
* Whack leading "[Tt]he",
*/
- if (strncmp(ostring, "The ", 4) == 0 ||
- strncmp(ostring, "the ", 4) == 0) {
+ if (( strlen(ostring) > target_len + 4) &&
+ (strncmp(ostring, "The ", 4) == 0 ||
+ strncmp(ostring, "the ", 4) == 0)) {
nstring = strdup(ostring + 4);
if (!nstring) {
fatal(needmem);
free(ostring);
ostring = nstring;
- /*
- * Eliminate Whitespace
- */
- tstring = strdup(ostring);
- if (!tstring) {
- fatal(needmem);
- }
- l = strlen (tstring);
- cp = ostring;
- for (i=0;i<l;i++) {
- if (!isspace(tstring[i])) {
- if (mustupper) {
- tstring[i] = toupper(tstring[i]);
+ if (!whitespaceok) {
+ /*
+ * Eliminate Whitespace
+ */
+ tstring = strdup(ostring);
+ if (!tstring) {
+ abort();
+ }
+ l = strlen (tstring);
+ cp = ostring;
+ for (i=0;i<l;i++) {
+ if (!isspace(tstring[i])) {
+ if (mustupper) {
+ tstring[i] = toupper(tstring[i]);
+ }
+ *cp++ = tstring[i];
}
- *cp++ = tstring[i];
}
+ free(tstring);
+ *cp = 0;
}
- free(tstring);
- *cp = 0;
/*
* Eliminate chars on the blacklist.
#include <ctype.h>
#include <math.h> /* for M_PI */
+#ifndef M_PI
+# define M_PI 3.141592653589
+#endif
+
#define MYNAME "PSP"
#define MAXPSPSTRINGSIZE 256
extern ff_vecs_t cetus_vecs;
extern ff_vecs_t gpspilot_vecs;
extern ff_vecs_t psp_vecs;
+extern ff_vecs_t garmin_vecs;
extern ff_vecs_t mxf_vecs;
extern ff_vecs_t holux_vecs;
"gpspilot",
"GPSPilot Tracker for Palm/OS"
},
+ {
+ &garmin_vecs,
+ "garmin",
+ "Garmin serial protocol"
+ },
{
&mxf_vecs,
"mxf",
projects / gpsbabel.git / commitdiff